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FOREWORD 


This  document  is  essentially  a  notebook,  recording  the  work  done  and  information 
acquired  during  a  two-phase  experiment  to  test  the  potential  application  of  fuzzy  logic  to 
the  Semi-Automated  Forces  (SAFOR)  system.  SAFOR  is  used  in  Simulation  Network 
(SIMNET)  battle  exercises  to  reduce  the  number  of  real  human  participants  by  replacing 
some  of  diem  with  virtual  participants  generated  by  computer  software.  A  major  objective 
of  SAFOR  is  to  replicate  human  behavior  with  enough  fidelity  that  a  human  taking  part  in 
an  exercise  will  be  unable  to  distinguish  between  simulated  objects  controlled  by  computer 
and  those  controlled  by  other  humans.  The  idea  underlying  the  use  of  fuzzy  logic  is  that 
algorithms  based  on  it  are  more  likely  to  resemble  the  kind  of  rough-estimate,  semi- 
qualitative,  mental  process  that  humans  apply  to  quick  decision  making  than  the 
madiematically  precise  algorithms  that  are  more  typical  of  computer  programs. 

The  question  addressed  in  this  experiment  is  whether  fuzzy  logic  can  be  a  general 
tool  for  improving  an  algorithm  affecting  the  action  of  one  or  more  SAFOR  entities  by 
making  the  action  resemble  human  behavior  more  than  it  would  have  without  the 
inqirovement  Therefore,  in  the  context  of  the  experiment  it  is  important  to  avoid  changing 
such  an  algorithm  in  any  way  odier  than  by  substituting  fuzzy  rules  for  precise  calculations 
that  would  normally  govern  a  particular  action.  An  important  objective  has  been  to 
demonstrate  that  this  can  be  achieved  in  a  straightforward  way  by  means  of  what  is  now  a 
standard  procedure  in  the  design  of  fuzzy  rule-based  control  systems. 
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ABSTRACT 


This  paper  describes  the  history  of  an  experiment  to  demonstrate  the  potential  utility 
of  fuzzy  logic  applied  to  SAFOR  algorithms  by  using  fuzzy  logic  to  modify  an  existing 
vehicle  collision  avoidance  algorithm.  Appended  to  the  paper  are  instructions  on  how  to 
create  SAFOR  exercises  on  a  Silicon  Graphics  terminal  in  the  IDA  Simulation  Center 
network.  This  report  also  contains  listings  of  source  codes  used  in  various  phases  of  the 
work. 
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I.  INTRODUCTION 


A.  BACKGROUND 
1.  SAPOR 

Reference  1  describes  SIMNET  as  a  joint  ARPA  and  U.S.  Anny  research  project 
with  the  goal  of  developing  the  technology  to  build  an  extended  network  of  interactive 
combat  simulators.  SAPOR  is  the  term  used  for  the  SIMNET  Semi-Automated  Forces 
system,  the  purpose  of  which,  as  stated  in  Ref.  1,  is  to  allow  "a  few  individuals  to  direct  a 
large  number  of  ground  and  air  vehicles  that  operate  as  a  unit  in  the  simulated  worid." 

Most  of  the  individual  SAPOR  vehicle  behavior,  e.g.,  avoiding  obstacles  or 
maneuvering  to  stay  in  a  prescribed  formation,  is  under  the  control  of  software  algorithms 
rather  than  humans.  On  the  other  hand,  it  is  hi^y  desirable  that  the  human  participants  in 
a  simulated  combat  exercise  not  be  able  to  distinguish  between  simulated  vehicles 
ctmtrolled  by  other  humans  and  those  that  are  con^uter  driven. 

In  various  recorded  SIMNET  exercises  deviations  from  realistic  vehicle  behavior 
range  from  the  subtle,  recognizable  only  by  experienced  observers,  to  the  gross  or  even 
bizarre.  Exanqrles  of  the  first  type  are:  uimecessarily  leaving  a  road  while  traveling  along 
it,  not  moving  realistically  over  certain  terrain  profiles,  not  changing  speed  when  climbing  a 
hill,  not  slowing  down  when  encountering  another  vehicle.  Examples  of  the  second  are: 
colliding  with  other  vehicles,  driving  into  a  lake,  going  through  a  wall.  Bizarre  behavior 
occurring  in  early  exercises  included  several  tanks  coalescing  to  form  an  unrecognizable 
mass  and  tanks  milling  about  randomly  before  lining  up  In  a  formation.  ^  Recent  examples 
are  two  or  nxne  vehicles  performing  a  kind  of  dance  and  single  vehicles  aimlessly  turning. 


^  These  "monster  vehicles”  are  really  not  due  to  a  SAFOR  deficiency,  but  the  results  of  initial  attempts 
to  de-aggregate  a  platoon  from  BBS  (an  algorithmic  war  game  that  operates  at  the  unit  level — platoon 
being  the  lowest  level)  into  its  individual  component  entities  (4  tanks)  in  SIMNET.  Sometimes  the 
de-aggregation  algmithm  fails  to  work  properly. 
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2 .  Fuzxy  Logic 

In  1965  L.  Zadeh,  a  professor  of  electrical  engineering  at  the  University  of 
California  whose  special  interest  was  in  the  design  of  control  systems,  introduced  the 
concept  of  fuzzy  sets  (Ref.  2)  as  a  generalization  of  classical  set  theory  to  deal  with 
linguistically  imprecise  notions  in  a  technologically  sound  way.  His  point  was  that  to 
perform  satisfactorily,  control  systems  need  not  be  optimal  or  even  based  on  complete 
knowledge  of  the  pertinent  physical  laws  of  motion.  In  fact,  he  expected  controls  designed 
using  fuzzy  sets  and  associated  rules  of  fuzzy  logic  to  mimic  human  behavior  when  applied 
to  ordinary  hiunan  activities  such  as  parallel  parking  a  car. 

With  the  advent  of  the  microcomputer  chip,  industrial  applications  of  21adeh's 
approach  became  feasible  and,  spearheaded  by  Japanese  manufacturers,  have  multiplied  at 
an  ever  increasing  rate  over  the  last  decade.  It  is  now  common  knowledge  that  the  fuzzy 
logic  methodology  in  conjunction  with  microcomputer  chips  has  resulted  in  many 
consumer-oriented  devices  that  exercise  human-like  judgment,  such  as  a  self-focusing 
camcorder  that  also  selects  the  focal  region  of  interest.  Equally  well  known  are  more 
elaborate  applications,  such  as  a  completely  automated  cement  kiln  or  subway  train,  in 
which  a  fuzzy  controller  replaces  a  human  operatcn:. 

The  success  of  fuzzy  logic  as  a  technique  for  imitating  human  behavior  in  the 
design  of  control  systems  suggests  that  the  same  approach  may  accomplish  a  similar  end  if 
applied  to  dynamic  computer  generated  simulations  like  those  created  by  the  SAPOR 
system.  The  objective  of  the  work  reported  in  this  document  has  been  to  demonstrate  the 
potential  utility  of  applying  fuzzy  logic  to  SAPOR  to  make  its  computer-controlled  entities 
behave  more  as  if  they  were  controlled  by  humans.  To  keep  the  demonstratitm  as  simple  as 
possible  the  work  has  conc'^ntrated  on  a  single  activity — avoiding  collisions  between 
vehicles. 

B .  PROJECT  HISTORY 
1 .  Perspective 

The  project  has  had  two  distinct  phases.  The  early  phase  concentrated  on  finding 
the  relevant  SAPOR  source  code.  Although  we  had  no  documentation  describing  the  actual 
SAPOR  algorithms,  and  therefore  no  way  of  knowing  what  they  were,  during  this  period 
we  also  considered  possible  collision  avoidance  algorithms.  The  later  phase  involved 
experimenting  with  SAPOR  by  creating  simple  exercises  in  which  vehicle  collisions 
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occurred  and  then  replacing  the  existing  avoidance  algorithms  by  fuzzy  versions  that 
avoided  collisions  in  identical  scenarios. 

2.  Phase  One 

While  still  ignorant  of  the  SAPOR  collision  avoidance  algorithms  we  attempted  to 
develop  some  of  our  own.  We  started  with  a  three-dimensional  concept  to  leave  open  the 
possibility  of  including  aircraft,  or  at  least  helicopter,  avoidance  maneuvering.  The  vehicle 
and  all  obstacles  were  represented  by  spheres. 

At  first  we  treated  ground  vehicles  as  spheres  resting  on  the  ground,  which  was 
assumed  to  be  a  plane  surface.  Later  we  transformed  the  algorithms  into  two-dimensional 
versions  that  use  cylindrical  instead  of  spherical  entities  for  representing  the  vehicle  and 
obstacles. 

Chapter  n  provides  a  description  of  the  nonfuzzy  (also  called  crisp)  algorithms 
along  with  a  detailed  analysis  deriving  the  necessary  supporting  mathematical  formulae. 
Deleting  the  z  coordinate,  which  represents  height  above  the  ground,  accomplishes  the 
transfonnation  of  the  formulae  from  the  spherical  to  the  cylindrical  geometry.  Chapter  m 
provides  a  brief  summary  of  the  fiizzy  logic  approach  to  algorithm  design  in  general  and  its 
application  to  obstacle  avoidance  by  a  vehicle  in  particular. 

The  approaches  to  collision  avoidance  in  Chapters  II  and  III  also  differ  in  another 
respect:  the  crisp  algorithms  take  into  account  all  obstacles  that  the  vehicle  can  encounter 
on  its  way  to  its  destination,  while  the  fuzzy  ones  consider  at  most  two  neighboring 
obstacles  at  a  time.  The  objective  in  the  first  case  was  to  devise  a  crisp  algorithm  to  define 
a  path  from  the  irutial  position  of  the  vehicle  to  its  destinatitxi  such  that  it  would  not  collide 
with  any  obstacle  along  the  way.  The  objective  in  the  second  case  was  to  design  a  fuzzy 
algorithm  that  would  behave  mote  naturally,  avoiding  obstacles  as  they  come  into  view 
without  necessarily  knowing  their  locations  in  advance.  In  both  cases  the  algorithms 
chosen  were  the  simplest  possible  that  appeared  to  meet  the  stated  conditicms. 

Both  the  crisp  and  the  fuzzy  algorithms  were  implemented  in  PC  GW  Basic 
programs  that  display  the  spherical  or  cylindrical  vehicle  avoiding  similarly  shaped 
obstacles  on  its  way  as  it  moves  from  the  left  end  of  a  PC  terminal  screen  to  the  right  end. 
Appendix  A  contains  figures  illustrating  the  path  of  the  vehicle  as  it  avoids  obstacles  in  five 
different  configurations,  using  both  the  crisp  and  fuzzy  algorithms,  for  both  the  spherical 
and  the  cylindrical  geometries.  Appendix  D  contains  listings  of  all  of  the  GW  Basic 
programs,  including  one  called  REPLOT.BAS,  which  replots  the  vehicle  path  as  shown  in 
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the  figures.  Also  included  in  the  appendix  are  Hies  containing  data  that  the  programs  can 
use  to  repiTxluce  the  exercises  associated  with  the  figures. 

3.  Phase  2 

The  second  phase  began  when  Kevin  Brown,  who  was  working  on  a  different 
SIMNET  programming  problem  but  knew  of  our  interest  in  SAPOR  collision  avoidance, 
located  a  Hie  called  avoid.c  containing  the  source  code  for  the  C  language  function 
avoid_a_vehicle  that  implements  avoidance  of  a  collision  between  two  vehicles.  That  file  is 
located  in  a  Silicon  Graphics  subdirectory  along  with  a  large  number  of  other  files 
containing  source  code  and  data  required  for  SAPOR.  Thus,  a  search  of  the  other  sub¬ 
directory  files  fm-  relevant  function  names  found  in  avoid.c  made  it  possible  to  uncover 
enough  source  code  to  completely  implement  the  SAPOR  vehicle  collision  avoidance 
algorithms. 

Chapter  IV  identifies  the  location  and  discusses  the  namre  of  the  source  code  for 
avoiding  vehicle  collisions  in  general  that  the  search  uncovered.  It  also  considers  in  detail 
the  calculations  made  to  implement  the  function  avoid_a_vehicle,  which  pertains 
specifically  to  die  avoidance  of  collisions  between  two  vehicles. 

At  this  point  it  was  clear  that  our  preliminary  work  on  avoidance  algorithms 
described  in  Chapters  n  and  III  had  concentrated  on  an  essentially  different  problem  than 
the  SAPOR  algorithms  address.  We  had  been  concerned  with  avoiding  multiple  obstacles, 
whereas  the  comparable  SAPOR  algorithms  could  only  deal  with  a  vehicle  against  a  single 
other  vehicle  in  uie  role  of  an  obstacle.  In  fact,  experimenting  with  a  scenario  involving 
two  stationary  tanks  near  each  other  acting  as  obstacles  and  a  third  attempting  to  avoid 
them,  demonstrated  that  the  moving  tank  may  avoid  one  of  the  stationary  ones  but  collide 
with  the  other. 

The  calculations  of  Chapter  FV  indicate  that  the  approach  of  the  SAPOR  function 
avoid_a_vehicle  is  actually  closer  to  that  of  our  fuzzy  than  our  crisp  algorithm  because  it 
deals  with  just  one  obstacle  at  a  time.^  This  made  the  insertion  of  a  fuzzy  version  into 
SAPOR  much  simpler  than  it  might  otherwise  have  been.  Qiapter  V  describes  in  detail  the 
new  algorithm  and  how  it  fits  into  avoid_a_vehicle.  Pigures  21  and  22  in  Appendix  A 
illustrate  the  difference  between  the  paths  that  a  cylindrical  vehicle  takes,  controlled  first  by 


^  In  fact,  it  does  not  seem  possible  to  use  our  crisp  algorithm  when  the  obstacles,  themselves,  are  < 

moving  vehicles. 
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the  regular  SAFOR  algorithm  and  then  by  the  fuzzy  algorithm,  when  avoiding  two 
cylindrical  objects. 

As  remarked  in  Chapter  V,  when  a  vehicle  is  turning  into  the  path  of  another  the 
tegular  SAFOR  algorithm  can  fail  to  prevent  a  collision,  while  the  fuzzy  version  averts  it 
when  applied  to  exactly  the  same  scenario.  Also  discussed  in  Chapter  V  is  a  discovery 
made  after  experimenting  with  various  exercises  using  either  the  crisp  or  the  fuzzy  version: 
causing  vehicles  to  slow  down  when  starting  an  avoidance  maneuver  greatly  enhances  the 
avoidance  process. 

Chapter  VI  describes  a  video  tape  recording  of  SAFOR  anomalies  observed  in 
several  exercises  displayed  on  the  SIMNET  Stealth  screen.  The  tape  also  shows  vehicle 
collision  scenes  with  the  crisp  SAFOR  avoidance  algorithm  in  control,  followed  by  their 
counterparts  obtained  by  recreating  the  ori^nal  scenes  with  the  fuzzy  algorithm  in  control. 

To  create,  save,  and  replay  SAFOR  exercises  it  was  necessary  to  use  the  SIMNET 
Silicon  Graphics  terminals.  Although  user's  manuals  for  some  versions  of  SAFOR 
(cf.  Ref.  3)  have  been  written,  at  the  time  we  were  unaware  that  any  existed.  Fortunately, 
the  SIMNET  staff  were  very  helpful,  so  that  learning  the  procedures  by  word  of  mouth 
was  not  as  difficult  as  it  might  seem.  Nevertheless,  we  hope  that  Appendix  B,  which 
outlines  the  basic  operational  steps  required  to  cany  out  the  experiments  discussed  in  this 
report,  can  still  have  some  value  as  a  primitive  guide  for  others  who  may  be  involved  in 
similar  programs  until  a  more  polished  and  con^hensive  manual  becomes  available. 
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11.  CRISP  OBSTACLE  AVOIDANCE 


^  A.  PRELIMINARY  ANALYSIS 

Assume  the  vehicle,  which  is  a  sphere  of  radius  Ry.  is  located  initially  at  the 
position  TV  and  that  its  final  destination  is  at  td-  Assume  that  N  obstacles,  which  are  also 
spheres  of  radius  Ro,  are  located  at  the  positions  rn.  Assume,  also,  that  the  vehicle  intends 

#  to  move  altmg  the  straight  path  connecting  ry  and  rt>-  In  dealing  with  obstacle  avoidance  it 
is  convenient  to  regard  all  vectors  as  relative  to  the  destination  point  rn,  working  with 
Ar  =  r  -  rD  instead  of  r.  Figure  1  illustrates  an  example  of  the  configuration  with  a  single 
obstacle  and  also  shows  die  radius  vector  rcn  to  the  point  on  the  vehicle  path  closest  to  the 

#  obstacle. 


Points  r  on  die  path  must  satisfy  the  equation 

ATvXAtsO  . 


(1) 
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From  (1)  it  follows  that 

Atv  X  (ATv  X  At)  =  (Afy  ■  Ar)Arv  -  jAryf  Ar  =  0  , 


so  that 


(Ary  At) 


Afy 


If  roi  is  the  point  on  the  path  closest  to  the  obstacle  located  at  rn  then  rn  must  satisfy 
Ary  (ArQ,-Ar.)  =  Ary  (rc-rJ  =  0  , 


sodtat 


But  because  of  (2) 


Ary  Arc.=Ary  Ar.  . 


.  ATy-ATo,  . 
Aro.=-i7~il^Ary  . 

IATvI 


It  then  follows  from  (3)  and  (4)  that 


It  follows  from  (5)  that 


.  Afy-Ar. 

^Cn  ”  I A  " 

jArvl 


The  distance  Dnbetween  the  point  on  the  path  rcn  closest  to  the  object  center  rn  and 
the  ofcgect  center  is  given 

=K-^Qif  =Kf-2Ar.-Arc„+|Arc„f  . 

Because  of  (S)  and  (6)  this  becomes 

•  C7) 


The  condition  that  the  vehicle  moving  along  the  path  will  collide  with  obstacle  n  is 

Di|S(R,  +  R.)’  .  (8 


The  distance  Dcn  between  rcn  and  the  position  of  the  vehicle  center  rvn  on  the  path  when 
the  ccdlision  occurs  is  given  by 


(9) 


The  collision  occurs  M^en  the  vehicle  position  on  the  path  is  given  by 

ATv 


Arv.=Aro.+Dc 


K| 


(10) 


where  Arcn  is  given  by  (5)  and  Dcn  is  given  by  (9). 

In  general  a  minimum  i^proach  sphere,  with  a  radius  R  »  Ry  +  Rn,  is  centered  at 
rn.  When  the  vehicle  comes  in  contact  with  the  obstacle  at  rn  its  center  lies  on  the  minimum 
i^roach  sphere  at  the  point  rvn.  obtained  by  adding  n)  to  Arvn  given  by  (10).  To  avoid 
being  impeded  by  the  obstacle  the  vehicle  can  move  around  it,  in  either  of  two  directicms, 
until  it  reaches  the  pc^t  where  it  touches  the  cone  that  has  its  vertex  at  ro  and  is  tangent  to 
the  minimum  approach  sphere.  The  vehicle  center  will  then  lie  on  a  conical  generator 
tangent  to  that  sphere  and  can  move  along  the  generator  toward  the  conical  ^x,  which 
coincides  with  the  destination  at  id.  Figure  2  illustrates  an  example  of  such  a 
cmifiguration,  in  vdiich  an  obstacle  directly  blocks  the  vehicle's  path  to  the  destination. 
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The  cone  is  tangent  to  the  sphere  at  a  circle  lying  in  a  plane  P  that  intersects  the 
conical  axis  at  the  point  ip,  given  by 


R*  |ArP-R* 

KT"'- 


(11) 


The  equation 


Ar-Arp=|Arpf  , 


which,  because  of  (1 1)  takes  the  form 


At -Alp 


(Kf-R’f 


(12) 


detomines  the  plane  P,  whose  intersection  with  the  sphere,  determined  by 

|Ar-Ar.f  =  R"  ,  (13) 

determines  the  tangent  circle.  Substitution  of  (1 1)  into  (12)  leads  to 

Ar.  Ar  =  lAr,f-R*  .  (14) 

The  combination  of  (13)  and  (14)  leads  to 

|Arf=|Ar.f-R*  ,  (15) 

which  ptnnts  on  the  circle  must  also  satisfy. 

Equatitms  (14)  and  (IS),  which  together  determine  the  circle  where  the  cone  and 
^hete  are  tangent,  are  equivalent  to 

Ar„-Ar*L*  , 

lArf  =  L*  ,  (16) 

where  L  is  the  distance  from  a  point  on  the  tangent  circle  to  the  destination  at  the  conical 
apex.  For  the  case  in  which  the  vehicle  and  obstacle  are  both  touching  the  plane  z  =  0,  i.e., 
ate  on  the  ground,  the  pair  of  equations  (16)  determine  two  points  at  the  intersection  of  die 
tangent  circle  with  the  plane  z  =  zd  =  zv-  This  is  because  the  position  of  the  vehicle  (or, 
for  that  matter,  of  the  obstacle)  refers  to  the  center  of  the  sphere  representing  it.  Then 
Az  =  0. 


If  for  the  vector  Arn  the  component  Axn  =  0,  the  Ax  and  Ay  components  of  the 
solution  At  of  (16)  are  given  by 
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(17a) 


I 


Ax  =  -  Ay*  , 


and  if  for  the  vector  Axn  the  (xxnponent  Ayn  «  0  the  Ax  and  Ay  components  of  the  solution 
At  can  be  obtained  by  interchanging  Ax  and  Ay  as  well  as  Axn  and  Ayn  in  (17a).  Other¬ 
wise.  those  components  are  pven  by 


Ax  = 


L»Ax.  ±  +Ayl)-L' 

AxJ  +  Ay.’ 


t 


Ay  = 


L?  -  Ax,  Ax 

Ay. 


(17b) 


Since  two  points  satisfy  (16)  the  vehicle  can  choose  between  two  possible  new 
paths.  A  single,  but  not  unreasonable,  choice  is  the  one  requiring  the  vehicle  to  travel  the 
least  distance  to  its  destination,  without  taking  into  account  future  collisions  with  other 
obstacles.  Fen-  this  purpose  it  is  tnily  necessary  to  select  the  point  determined  by  the  vector 
Ar  for  which  the  distance  Svd  given  by 

Svd=K-^+M  (1*) 

has  the  smaller  of  the  two  possible  values. 

If  a  pair  of  (^herical)  obstacles  are  too  close  together  they  can  be  combined  into 
one  large  sphere  enclosing  them  both.  For  deciding  when  they  are  too  close,  a  safe  (but 
perhaps  too  stringent)^  criterion  would  be:  when  the  shortest  distance  between  the  two 
obstacle  surfaces  is  less  than  the  vehicle  diameter,  Le.,  if  the  condition 

+  (19) 

is  satisfied.  If  (19)  is  satisfied,  an  obstacle  with  the  radius  Rgm,  given  by 

R-  * ^(|r.  R.  +  R.)  .  (20a) 

will  take  the  place  of  the  obstacles  centered  at  rm  and  rn  with  radii  Rm  and  Rn.  The  center 
of  the  replacement  will  be  located  by  the  vector 


^  In  fact  if  the  vehicle  and  obstacles  are  on  the  ground  a  straightforward  geometrical  raimiatinn 
that,  instead  of  (19).  the  least  stringent  condition  is 
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which  is  equivalent  to 


r  = 


(20b) 


B .  A  CRISP  AVOIDANCE  ALGORITHM  FOR  A  VEHICLE  AND 
OBSTACLES  ON  THE  GROUND 

The  basic  idea  of  this  algoridun  is  always  to  move  the  vehicle  in  a  straight  line  from 
its  present  position  toward  its  final  destination  whenever  possible.  Using  (8),  the 
algorithm  determines  the  nearest  obstacle  with  which  the  vehicle  will  collide  if  it  continues 
on  that  path.  Then  the  next  step  is  to  find  the  collision  point,  using  (10),  and  determine 
whether  the  vehicle  can  go  around  the  obstacle  on  a  circular  path  to  a  position,  calculated 
using  (17a)  or  (17b)  along  with  (18),  from  which  it  can  proceed  in  a  straight  line  toward 
the  destination.  This  will  be  possible  if  the  distance,  determined  from  (19),  between  the 
nearest  obstacle  and  all  others,  or  all  others  but  one,  is  large  enough;  then  if  it  is  obstructed 
cm  one  side  the  vehicle  can  go  around  the  merest  obstacle  on  the  other  side. 

If  more  than  one  other  obstacle  is  close  enough  to  the  nearest  one  to  block  the 
vehicle,  the  algorithm  assumes  that  the  blockage  occurs  for  either  path  around  the  nearest 
obstacle,  which  it  replaces,  along  with  one  of  those  that  are  too  close,  by  a  sphere  whose 
center  is  given  by  (2(tt>)  and  whose  radios  is  given  by  (20a).  The  sphere  encloses,  and  is 
tangent  to,  both  obstacles  bring  replaced. 

After  two  obstacles  have  been  replaced  by  a  single  sphae  in  diis  way  the  algorithm 
starts  over  from  the  beginning  with  the  new  configuration  of  spheres  replacing  the  old. 
This  procedure  ccmtinues  until  it  is  no  longer  necessary  to  replace  two  spheres,  regarded  as 
ringle  obstacles,  with  one;  i.e.,  the  vehicle  can  avoid  all  spheres  in  the  final  configuration, 
and  therefore  all  of  the  original  obstacles,  without  colliding  with  any. 


12 


in.  FUZZY  OBSTACLE  AVOIDANCE 


A.  PRELIMINARY  DEFINITIONS 

In  fuzzy  logic  terminology  it  is  customary  to  refer  to  a  set  as  defined  in  classical 
logic  as  "cti^.”  Associated  with  any  set  in  classical  logic  is  its  "characteristic  function," 
(tefined  over  all  elements  in  the  universe  of  discourse  (i.e.,  all  elements  that  could  possibly 
be  members  of  the  set)  as  having  the  value  1  for  every  element  that  is  a  member  of  the  set 
and  the  value  0  for  every  one  that  is  not.  Associated  with  every  "fuzzy”  set  is  its 
"membership  function,"  which  for  every  element  in  the  universe  of  discourse  has  a  value  in 
the  closed  interval  from  0  to  1.  The  crisp  set  characteristic  function  is  obviously  a  special 
case  of  the  fuzzy  set  membership  function. 

Here  fuzzy  obstacle  avoidance  means  using  an  algorithm  consisting  of  some  rules 
expressed  in  the  modens  ponens  form  "If  A  tiien  B,"  where  A  and  B  define  fuzzy  sets.  Fot 
exanqile,  A  might  be  the  phrase  "the  distance  to  the  obstacle  is  large"  and  B  the  statement 
"the  change  in  the  direction  of  the  vehicle  motion  is  small."  The  phrases  "lar^  distance  to 
the  obstacle"  and  "small  change  in  the  ditectimi  of  the  vehicle  motion"  refer  to  fuzzy  sets 
defined  by  specific  membership  functions. 

The  membership  function  of  the  set  "large  distance  to  the  obstacle"  might  be  a  non¬ 
decreasing  function  that  is  0  as  the  obstacle  distance  increases  from  0  to  50  meters, 
increases  linearly  fimn  0  to  1  as  the  obstacle  distance  increases  from  50  to  100  meters,  and 
remains  equal  to  1  for  all  obstacle  distances  larger  than  100  meters.  Figure  3  illustrates  a 
simple  piecewise  linear  membership  function  for  the  fuzzy  set  "large  distance  to  the 
obstacle." 

Membership  functions  for  "medium  distance  to  the  obstacle"  and  "small  distance  to 
the  obstacle"  can  be  defined  in  a  similar  way.  Figure  4  illustrates  one  for  "medium  distance 
to  the  obstacle,"  and  Hgure  5  illustrates  (xie  for  "small  distance  to  the  obstacle". 
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Figure  3.  Fuzzy  Set  Membership  Function~Large  Distance  to  the  Obstacie 


Figure  4.  Fuzzy  Set  Membership  Function-Medium  Distance  to  the  Obstacie 


Figure  5.  Fuzzy  Set  Membership  Function-Smaii  Distance  to  the  Obstacle 
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Three  parameters  Pi.  P2.  P3  are  sufficient  to  characterize  any  function  of  the  types 
illustrated  in  Figures  3-S.  The  following  rules  define  a  generic  membership  function 
representing  smaiU  medium,  or  large. 

If 

P,<P2  . 


then  the  fiizzy  set  is  large  and  its  membership  function  is  defined  by: 


P^PJ. 

MF(P)  =  1; 

Pjsp>p„ 

MF(P)  =  :^ 
*2 

p,^p. 

MF(P)  =  0. 

If 

Pj<Pj<Pj  , 

then  the  fuzzy  set  is  medium  and  its  membership  function  is  defined 

fP<P,.  MF(P)  =  0; 

P-P, 


P,<P<Pj.  MF(P)  = 


Pj^P<P,.  MF(P)  =  -^ 

P3<P. 


P2-P.’ 

P,-P 


P3-P2 

MF(P)  =  0. 


If 


P2  <  P|  , 

then  the  fuzzy  set  is  small  and  its  membership  function  is  defined  by: 


P^Pj, 

MF(P)  =  1; 

P2^P<P3. 

P  — P 

MF(P)=-p-^; 

P3-P2 

P3SP. 

II 

0 
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The  membership  function  ^aobCP)  of  the  intersection  of  two  fuzzy  sets  A  and 
B  with  membership  functions  |J.a(P)  and  ^b(P)  ia  given  by  the  function,  which  for  each 
value  of  P  is  equal  to  the  smaller  of  the  two  membership  functions  at  that  value,  i.e., 

^^^(P)  =  min[n^(P),MB(P)]  .  (21) 

For  example,  the  intersection  of  "large  distance  to  the  obstacle"  with  "medium  distance  to 
the  obstacle,"  the  curves  of  whose  membership  functions  appear  separately  in  Figures  3 
and  4  and  together  in  Figure  6,  has  a  membership  function  depicted  by  the  curve  in 
Figure  7. 


Figure  6.  Fuzzy  Set  Membership  Functions~Medium  Distance 
to  the  Obstacie  and  Large  Distance  to  the  Obstacie 


Figure  7.  Fuzzy  Set  Membership  Function-intersection  Between 
Medium  Distance  and  Large  Distance  to  the  Obstacie 
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The  memb»ship  function  PaubCP)  fof  the  union  of  two  fuzzy  sets  with  member¬ 
ship  functions  Pa(P)  and  iisCP)  ts  given  by  the  function,  which  for  each  value  of  P  is  equal 
to  the  larger  of  the  two  membership  functions  at  that  value,  i.e., 

4^,.(P)  =  max[n^(P).43(P)]  .  (22) 

The  curve  in  Figure  8  depicts  the  membership  function  of  the  union  of  the  two  fuzzy  sets 
whose  membership  function  curves  appear  in  Figure  6. 


Plgura  8.  Fuzzy  Set  Membership  Functlon~Unlon  of  Medium 
Distance  and  Large  Distance  to  the  Obstacle 

A  rule  of  the  form  "If  A  then  B"  is  a  fuzzy  relation.  The  elements  of  the  sets  to 
which  the  statements  A  and  B  refer  are,  in  general,  from  different  universes  of  discourse. 
For  example,  suppose  that  A  represents  the  phrase  "the  distance  to  the  obstacle  is  large" 
and  B  the  phrase  "add  a  small  velocity  increment"  Clearly,  no  element  in  the  set  of  Targe 
distances  to  die  obstacle,"  to  which  the  premise  A  refers,  can  be  a  candidate  for  inclu^n  in 
the  set  of  "small  velocity  increments,"  or  vice  versa.  However,  the  elements  in  the  set  of 
"large  distances  to  the  obstacle"  and  the  elements  of  the  set  of  "small  velocity  increments," 
although  from  different  universes  of  discourse,  exist  simultaneously  in  the  cartesian 
product  of  the  two  universes  of  discourse,  which  is  a  two-dimensional  space,  representing 
a  combined  universe  of  discourse  wherein  the  elements  of  the  sets  referenced  by  A  and  B 
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belong  to  mutually  (Hthogonal  cylindrical  sets.^  The  complex  set  referenced  by  the  relaticm 
"if  A  then  B"  is  the  intersection  of  the  two  cylindrical  sets  in  the  product  space. 

In  the  classical  case  the  intersection  can  be  characterized  completely  by  a  two- 
dimensional  characteristic  function:  defined  as  1  for  every  pair  of  elements  (a,b)  such  that 
a  is  in  the  set  referenced  by  A  and  b  is  in  the  set  referenced  by  B,  while  for  all  other  pairs 
the  characteristic  function  is  defined  as  0.  In  the  fuzzy  case,  of  interest  here,  the  two- 
dimensional  membership  function  |iAnB(P>Q)  is  defined  by 

=  (23) 

exactly  as  in  (21),  except  that  the  result  is  a  function  of  P  and  Q  instead  of  just  P. 

A  standard  fuzzy  set  coiiq)utational  procedure  called  con^sition  provides  a  method 
for  implementing  a  rule  such  as  "If  A  then  B"  in  an  algorithm.  Suppose  that  when  A  is  "the 
distance  to  the  obstacle  is  large"  and  B  is  "add  a  small  velocity  increment"  an  observed 
value  of  the  distance  to  the  obstacle  is  better  described  by  "the  distance  to  the  obstacle  is 
small"  Then,  given  the  membership  function  po(P)  of  the  observed  quantity,  composition 
determines  the  membership  function  liactionCQ)  of  the  quantity  associated  with  the  action 
implied  by  the  rule,  i.e.,  the  increment  that  should  be  added  to  the  velocity  in  place  of  the 
small  increment  that  the  rule  prescribes  when  the  distance  to  the  obstacle  is  large.  The 
cakulatitm  for  this  purpose  is 

^l«s-(Q)  =  ®aXp{*®n[^lo(P).4Ar^B(P♦Q)]}  *  (24) 

where  the  minimum  implied  by  "min”  is  the  smaller  of  tiie  two  membership  functions 
inside  the  square  brackets,  as  in  (21)  and  (23),  but  the  maximum  implied  by  "max"  is  the 
largest  value  of  that  result  for  all  possible  values  of  P. 

If  an  algcxithm  consists  of  more  than  one  "If  A  then  B"  rule,  the  action  fuzzy  set 
implied  by  the  whole  algorithm  is  the  union  of  all  of  those  defined  by  the  separate  rules. 
Thoefore,  the  membership  function  Palg(P)  associated  wnth  the  algorithm  can  be  calculated 
for  each  value  of  P  by  taking  the  largest  corresponding  value  of  all  action  membership 
functions  associated  with  the  separate  rules  and  calculated  using  (24);  i.e., 

H.^(P)  =  maXi[p^,(P)]  .  (25) 


^  By  definition,  sets  consisting  of  element  pairs  that  include  all  elements  from  one  of  the  original 
universes  of  discourse  are  cylindrical  sets. 


18 


where  Mectioo^CP)  is  the  membership  function  associated  with  the  action  fuzzy  set  defined 
by  rule  i. 


B .  FUZZY  OBSTACLE  AVOIDANCE  RULES 

It  is  assumed  here  that  in  avoiding  an  obstacle  a  vehicle  adds  an  increment  to  its 
own  velocity  vector,  which  is  assumed  to  be  constant  except  during  the  avoidance 
maneuver,  and  that  the  direction  of  the  increment  is  orthogonal  to  the  line  of  sight  between 
the  vehicle  and  the  obstacle.^  The  magnitude  of  the  increment  is  regarded  as  a  fuzzy  set 
depending  on  two  observable  fuzzy  sets:  the  distance  between  the  vehicle  and  the  obstacle 
(measured  frcHn  center  to  center)  and  the  obstacle's  radius. 

Table  1  defines  a  set  of  nine  rules  for  determining  die  velocity  increment  magnitude, 
given  the  observable  sets.  The  phrase  A  in  the  "If  A  then  B"  form  of  a  rule  in  this  case 
refers  to  a  complex  set:  the  intersecdon  of  an  "obstacle  distance  from  the  vehicle"  with  an 
"obstacle  radius."  Three  possibilities  exist  for  each  type  of  set:  small,  medium,  and  large. 

Table  1.  Fuzzy  Vehicle  Velocity  Avoidance  Increment  Dependence  On 
Obatacle  Radlua  and  Distance  from  Vehicle 


Distance 

From 

Vehicle 


Obstacle  Radius 


Small 

Medium 

Large 

SmaB 

Medium 

Large 

Large 

Medium 

Small 

Medium 

Large 

Large 

Small 

Smal 

Medium 

The  phrase  B  in  each  rule  applies  to  the  fuzzy  velocity  increment  size  that  appears  at  the 
intersection  of  the  row  and  column  in  the  table  labeled  in  accordance  with  the  fuzzy  sizes  of 
the  two  quantities  whose  intersection  is  specified  by  the  phrase  A  in  the  rule.  For  example, 
the  first  rule  given  by  Table  1,  at  the  intersection  of  row  1  with  column  1,  is:  "If  the 
distance  between  the  vehicle  and  the  obstacle  is  small  and  the  obstacle  radius  is  small,  then 
the  velocity  increntent  should  be  medium." 


^  An  ahonative,  used  in  the  current  S  AFCHt  avoidance  algorithm  when  the  obstacle  is  another  vehicle,  is 

to  make  the  increment  direction  orthogonal  to  the  vehicle's  own  velocity.  At  least  for  sphnical 
obstacles,  this  does  not  work  as  well  as  choosing  the  direction  to  be  orthogonal  to  the  line  of  sight,  as 
is  done  bm 
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Since  the  actual  calculations  will  generally  be  numerical,  although  the  membership 
functions  are  theoretically  continuous  functions  of  the  independent  variables,  the  actual 
values  used  for  both  the  independent  and  dependent  variables  will  be  limited  to  a  discrete, 
finite  set  Also,  in  the  end  even  a  fuzzy  algorithm  must  lead  to  a  single  number  to  be  useful 
(i.e.,  the  algorithm's  action  fuzzy  set  must  be  defuzzified).  The  most  popular  method  for 
accomplishing  diis  is  to  calculate  the  centroid  givot 

tiwpj 


P.4= 


where  Paig  is  the  value  of  the  action  quantity  recommended  by  the  algorithm,  e.g.,  the 
magnitude  of  an  increment  to  be  applied  to  a  vehicle's  velocity  in  the  case  of  a  fuzzy 
obstacle  avoidance  algorithm. 

C.  A  FUZZY  OBSTACLE  AVOIDANCE  ALGORITHM  FOR  A  VEHICLE 
AND  OBSTACLES  ON  THE  GROUND 

This  algorithm  uses  Table  1  to  calculate  die  magnitude  of  a  vector  increment  added 
to  the  vehicle  path  direction  to  avoid  a  collision  with  an  obstacle.  The  direction  of  the 
increment  is  orthogonal  to  the  line  through  the  centers  of  the  vehicle  and  the  obstacle. 

The  fuzzy  membership  functions  used  for  this  purpose  are  those  illustrated  in 
Figures  3-5  and  defined  earlier,  as  well  as  similar  membership  functions  for  fuzzy  obstacle 
radius  sizes.  Since,  as  in  the  case  of  the  "distance  from  obstacle"  sizes,  tiiey  are  all  of  the 
generic  type,  three  parameters  completely  specify  each  of  the  "obstacle  radius"  size 
monbership  functiems.  The  parameter  sets  are  as  follows: 

For  "small  radius"  Pi  =  35,  P2  *  5,  P3  =  20; 

For  "medium  radius"  Pi  =  5,  P2  =  20,  P3  =  35; 

For  "large  radius"  Pi  =  20,  P2  «  35,  P3  =  5. 

As  mentioned  earlier,  in  each  rule  of  the  form  "if  A  then  B"  A  is  the  intersection  of 
the  "distance  from  obstacle"  and  the  "obstacle  radius,"  which,  although  possibly  measured 
in  the  same  units,  are  different  types  of  quantities.  Therefore,  the  independent  variables 
must  be  different  in  their  respective  membership  functions,  from  which  it  follows  that  the 
membership  function  for  A  must  be  a  function  of  two  variables.  In  fact,  according  to  (23) 
in  rule  ij  it  must  be  given 
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(27) 


4a«(P.Q)  =  imn[n^(P).4^(Q)] 

where  M<ii(P)  is  the  membership  function  for  a  "distance  from  obstacle"  size  i  and  is 
the  membership  functi<Mi  for  an  "obstacle  radius"  size  j.  Similazly,  (23)  also  (tetermines  the 
membership  function  for  the  intersecdon  of  the  observed  "distance  from  obstacle"  and 
"obstacle  radius”  sizes,  so  that  the  result  MO(P>Q)  bas  the  same  form  as  that  given  by  (27); 

4o(P.Q)  =  inin[po,(P).lio,(Q)]  .  (28) 

where  Pod(P)  is  the  membership  function  fcH*  the  observed  "distance  from  obstacle"  size 
and  ltOr((^  is  the  membership  function  for  die  observed  "obstacle  radius"  size. 

If  the  "vector  increment"  magnitude,  which  is  B  in  the  rule,  has  a  membership 
function  liiiic4j(R)>  (23)  determines  the  rule  relation  membership  function  Pr(P>Q>P)> 
which  is  dien  given  by 

p^(P,Q,R)  =  min[p^(P,Q)p^.y(R)]  ,  (29) 

where  )XAij(P.Q)  is  given  by  (27).  Then  for  the  "vector  increment"  magnitude  specified  by 
the  rule  ij,  (24)  provides  the  membership  function  Paction4j(P)>  given 

^«i«.«(R)  =  «*axp.Q{rmn[iio(P.Q).li4i(P.Q.R)]}  .  (30) 

where  the  quantities  on  the  right  side  of  (30)  are  given  by  (28)  and  (29). 

After  calculating  all  of  the  individual  rule  membership  functions  Mection4j(R)  given 
by  (30),  the  next  step  is  calculate  the  single  velocity  increment  membership  functimi 
4inc(R)>  which  is  the  membership  function  fm-  the  union  of  the  sets  defined  by  all  of  the 
rules.  According  to  (25),  it  is  given  by 

Pi„(R)  =  maXjj[p^^(R)]  ,  (31) 

where  liij^tion(R)  is  given  by  (30). 

The  final  calculation  based  on  (26)  uses  piiic(R)  to  estimate  the  vector  increment 
magnitude.  The  result  is  given  by 
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where  ^incCR)  is  given  by  (31).  In  (32)  each  continuous  variable,  i.e.,  the  membership 
function  and  its  argument,  is  replaced  by  a  discrete  set  of  sample  values.  As  a  practical 
matter  this  must  be  done,  in  any  case,  before  numerical  results  can  be  obtained. 

When  two  or  more  obstacles  are  too  close  together  for  the  vehicle  to  pass  between 
them  the  fuzzy  algorithm  does  not  replace  tl^m  with  a  single  sphere  as  the  crisp  algorithm 
does.  Instead,  the  fuzzy  algorithm  proceeds  as  follows. 

If,  when  it  moves  in  the  normal  direction,  avoiding  one  obstacle,  the  vehicle  will 
collide  with  another,  the  algorithm  reverses  its  direction  for  one  incremental  path  distance 
and  then  has  it  attempt  to  resume  noimal  motion  toward  the  destination.  If  the  vehicle  will 
move  closer  to  the  last  obstacle  encountered  and  the  distance  between  the  last  obstacle  and 
the  nearest  one  ahead  is  too  small  to  permit  passage,  the  algorithm  reverses  the  vehicle's 
direction,  causes  it  to  move  the  minimum  incremental  distance,  and  then  continues  to  have 
it  proceed  toward  the  destination. 


IV.  SAFOR  OBSTACLE  AVOIDANCE 


A.  SAFOR  AVOIDANCE  IN  GENERAL 

Source  code  in  the  file  called  driver.c  governs  the  avoidance  of  obstacles  by 
vehicles  in  the  SAFOR  simulation.  The  functitms  used  by  code  in  driver.c  for  this  purpose 
depend  on  the  type  of  obstacle  to  be  avoided.  In  particular,  the  function  for  avoiding 
vehicles,  other  than  fixed  wing  aircraft  but  including  dismounted  infantry,  is  called 
avoid.vehicles,  and  its  source  code  can  be  found  in  avcntLc. 

The  SAFOR  function,  defined  by  source  code  in  driver.c,  for  avoiding  extended 
obstacles,  i.e.,  lakes,  rivers,  canopies,  buildings,  and  tree  lines,  is  avoid_objects.  That 
function  treats  any  of  those  obstacles  as  a  collection  of  line  segments.  It  avoids  them  by 
means  of  the  repetitive  use  of  the  function  called  avoid.line,  the  source  code  for  which  is 
also  in  driver.c. 

The  work  described  in  this  document  considers  cmly  the  SAFOR  avoidance  of 
ground  vehicles.  The  function  that  the  code  in  driver.c  uses  for  this  purpose, 
avoid.vehicles,  refers  to  another  function,  constant_velocity_avoid,  which  in  turn  refers  to 
a  third  fiincticm,  avoid_a_vehicle.^  The  source  code  for  each  of  these  functions  is  in  the 
file  avoid.c.'^ 

B .  SAFOR  VEHICLE  AVOIDANCE 

The  function  avoid_a_vehide  implements  the  basic  algorithm  for  collision  avoid¬ 
ance.  The  steps  involved  are  as  toUows. 

1 .  Calculate  the  2-dimensional  vector  rpos  representing  the  (instantaneous)  relative 
position  of  the  avoidee  vehicle  relative  to  the  avoider  by  subtracting  the 


^  The  sane  functions  also  apply  to  the  avoidance  of  helicopiefs.  However,  the  functkxi  f(x  avoiding  fixed 
wing  aiiciaft  is  more  elaborate  and  quite  difTetent  It  is  called  fixed  wing  avoid  and  its  souice  code  is 
inthefileavoidx. 

^  The  file  avokLc  also  contains  source  code  for  another  function  called  avoid.an.object,  nkich  closely 
lesemUes  av(HdjB_vehicle.  However,  a  search  of  the  appropriate  SAFOR  souim  code  files  (those  in 
Thsr/ufdeveVdeveloper/sr^host)  uncovered  no  reference  to  diat  function  by  any  other  function  in  the 
present  veisian  (SAP  43.3)  of  SAFOR,  indicating  that  the  system  no  longer  uses  it 
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2-diiiiensional  position  vector  of  the  avoider  from  that  of  the  avoidee  and 
correcting  for  the  difference  between  the  tick  times  associated  with  the  two 
vehicles. 

2.  Calculate  the  distance  distance_at_cpa  of  the  avoidee  from  the  closest  point 
of  approach  (cpa)  of  the  avoider.  This  is  done  by  fu-st  calculating  the 
2-dimensional  relative  velocity  vector  vki,  which  is  the  difference  between  the 
velocity  vector  of  the  avoider  and  that  of  the  avoidee,  and  then  calculating  the 
cross  product  of  that  relative  velocity  and  the  relative  position  divided  by  the 
relative  speed: 

v^  xr_ 

distance_at_cpa=  ^ 


Since  the  vectors  are  2-dimensional  their  cross  product  has  just  one  non-zero 
conqmnent,  which  is  orthogonal  to  the  plane  of  Vni  and  rpos.  and  can  therefore 
be  treated  as  a  scalar.  The  sign  of  its  value  distance_at_cpa  depends  on  which 
side  of  the  avoidee  the  relative  velocity  is  directed. 

3 .  If  the  actual  distance  that  is  equal  to  the  absolute  value  of  distance_at_cpa  is 
greater  than  a  threshold  called  Passing_Dist,  no  avoidance  maneuver  is 
required.  At  the  beginning  of  avoid.c  the  quantity  Passing_Dist  is  defined  as  5 
if  both  vehicles  are  dismounted  infantiy,  10  if  the  vehicles  are  on  the  ground 
and  at  least  one  of  them  is  not  dismounted  infantiy,  and  SO  if  the  vehicles  are 
helicopters.^ 

4.  If  avoidance  is  required,  calculate  the  distance  to  the  cpa,  which  is  the  absolute 
value  of  die  quantity  given 

..  •  r„ 

dist_to_cpa  =  — ; — P-  . 

ri««l 


If  dist_to_cpa  is  negative  the  avtnder  has  passed  the  avoidee,  so  that  avoidance 
is  no  longer  necessary.  If  not,  then  calculate  the  time  to  the  cpa,  which  is 
given  by 


time_to_cpa 


dist_to_cpa 

h'-l 


If  time_to_cpa  exceeds  a  threshold  called  GRND_L(X)K_AHEAD,  which  is 
defined  at  the  beginning  of  avoitLc  as  8,^  then  quit  because  the  obstacle  is  too 
far  away  to  require  an  avoidance  maneuver. 


^  Since  the  numbeis  refer  to  distances  the  units  are  presumed  to  be  meters. 

^  In  this  case  the  threshold  must  be  in  units  of  time,  which  are  assumed  to  be  seconds. 


24 


S.  If  there  is  a  destination  point,  determine  whether  the  quantity  called  speed, 
which  is  the  magnitude  of  the  avoider  velocity,  is  less  than  a  threshold  called 
SMALL,  which  is  defined  as  O.OOOS  at  the  beginning  of  a  header  file  called 
util.h.  If  it  is,  then  to  avoid  dividing  by  0  skip  the  avoidance  process; 
otherwise,  continue  by  calculating  the  vector  position  rpoint  of  the  destination 
relative  to  the  avoider  by  subtracting  the  avoider  position  vector  from  that  of 
the  destination.  Then  the  time  to  the  destination  point  is  given  by 

time_  to_  point  = 

speed 


6.  If 


tiine_to_point  <  time_to_cpa  , 


then  the  avoider  will  arrive  at  the  destination  before  the  collision,  the  avoidance 
of  which  is  therefore  unnecessary.  Otherwise,  calculate  a  quantity  called 
scale.factor,  given  by 


scale.factor  *  sgn(dist_at_q)a)  •  AVOID_  FACTOR  • 


Passing_Dist-|di5t_at_q>a| 
iiiax(time_  to_  qja,  MINIMUM.  TIME) 


The  quantities  MINIMUM.ITME,  which  limits  the  possible  size  of 
scale.factor,  and  AVOID_FACTOR  are  both  defined  as  0.5  at  the  beginning  of 
avoid.c. 


7 .  Defme  a  vector  increment  Av,  in  terms  of  the  unit  vector  Vq  orthogonal  to  the 
relative  velocity  Vid  of  the  avoider,  by 


Av  s  (scaie_fact(M')Vo 

where,  in  terms  of  the  components  (vi,v2)  of  Vrei,  the  components  (voi,Vo2)  of 
Vo  are  given  by 


Then  change  the  avdder  velocity  by  adding  the  increment  Av  to  it 
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V.  SAFOR  VEHICLE  AVOIDANCE  USING 
A  FUZZY  ALGORITHM 


A.  THE  CONCEPT 


As  observed  in  Section  IV.B,  the  present,  crisp,  SAFOR  algorithm  for  avoiding 
ground  vehicle  collisions  accofxq)lishes  this  by  adding  an  ordtogonal  vector  increment  to  the 
velocity  of  an  avoider  vehicle  relative  to  that  of  the  avoidee.  The  magnitude  of  the 
corrective  increment  is  a  scale  factor  based  on  the  time  to  the  cpa  and  a  fixed  passing 
distance  (pd)  threshold  of  10  meters. 

Experimenting  with  simple  exercises  involving  two  tanks  or  a  single  annored 
personnel  carrier  and  a  platoon  of  tanks  has  shown  that  collisions  can  occur  in  certain 
situations,  in  particular,  when  a  vehicle  is  turning  into  the  path  of  another  vehicle.  The 
reason  in  that  case  is  probably  a  consequence  of  the  algorithm’s  basic  assumption  that  all 
velocities  are  constant  Thus,  since  fuzzy  rules  in  algorithms  are  inherently  nmilinear  it  is 
not  surprising  that  in  exactly  the  same  scenarios  replacing  the  scale  factor  by  one  derived 
finom  fuzzy  rules  averted  tiie  collisions. 

The  idea  was  to  use  rules  like  those  defined  in  Table  1,  with  quantities  PD  and 
DISTANCE  TO  CPA  replacing  OBSTACLE  RADIUS  and  DISTANCE  FROM  VEHICLE. 
After  starting  with  some  common  sense  guesses,  to  establish  the  rules  a  procedure,  called 
"tuning"  in  die  fuzzy  logic  literature,  which  involves  testing  them  in  some  typical  exanqiles 
and  making  iqipropriate  changes  until  they  work  satisfactorUy,  can  be  used.  In  die  ctdlision 
experiment  starting  with  rules  corresponding  to  those  identified  with  Table  1  and  dien 
tuning  them  resulted  in  Table  2,  which,  in  fmt,  differs  only  slightly  from  Table  1. 


Table  2.  Fuzzy  Vehicle  Velocity  Avoidance  increment  Dependence  On 
Paaaino  Distance  and  Distance  to  Closest  Point  of  Approach 


Distance 

To 

Cpa 


PD 


Small 

Medium 

Large 

Smal 

Medium 

Large 

Large 

Medium 

Small 

Large 

Large 

Large 

Smafl 

MetSum 

Medium 
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B .  THE  IMPLEMENTATION 


It  was  also  necessary  to  apply  the  tuning  process  to  the  deHnition  of  the  small, 
medium,  and  large  membership  functions  associated  with  the  pd,  distance  to  cpa  (dcpa)  and 
scale  factor  (self),  each  of  which  was  assumed  to  be  of  the  generic  type  defined  in 
Section  niA  and  therefore  specified  by  three  parameters.  Actually,  it  is  only  necessary  to 
assign  three  parameters  to  each  of  the  quantity  types:  pd,  dcpa,  self;  the  order  of  the 
parameters  determines  whedier  the  txxKlifier  is  small,  medium,  or  large. 

The  quantity  pd  in  the  existing  crisp  version  of  the  SAPOR  avoidance  algmithm  is 
a.«tsigned  the  possible  values  5, 10,  or  SO,  depending  on  whether  the  avouter  and  avoidee 
vehicles  are  both  dismounted  infantry,  both  are  on  the  ground,  and  at  least  one  is  not 
dismounted  infantry,  or  they  are  both  helicopters.  Therefore,  a  natural  choice  of 
parameters  PDl,  PD2,  PD3  associated  with  the  fuzzy  pd  membership  function  seemed  to 
be:  PDl  =  0,  PD2  =  5,  PD3  =  10.  in  the  first  case;  PDl  =  5.  PD2  =  10,  PD3  =  50  in  the 
second  case;  PDl  =  10,  PD2  =  SO,  PD3  =  100  in  the  third  case.  For  the  parameters 
associated  with  the  dcpa  membership  function,  the  choice  for  ground  vehicles,  which  were 
the  only  type  actually  considered  in  the  experiment,  was:  DCPAl  =  10,  DCPA2  s  SO, 
DCPA3  =  100.  The  first  choice  for  self  was:  SCLFl  =  1,  SCXF2  =  3,  SCLF3  =  5. 
However,  it  was  quickly  observed  that  those  values  were  too  small,  but  that  doubling 
each  of  them  led  to  good  results;  i.e.,  the  final  choice  was:  SCLFl  =  2,  SCLF2  »  6, 
SCLF3  *  10. 

For  each  type  of  variable  the  associated  parameter  values  increase  in  die  same  order 
as  the  subscripts  on  the  symbols  representing  them.  Thus,  by  virtue  of  the  properties  of 
the  generic  membership  function  determined  a  set  of  three  parameters,  as  defined  in 
Section  HI. A,  the  membership  functions  determined  by  the  triplets  (PD3,  PDl,  PD2), 
(DCPA3,  DCPAl,  DCPA2),  (SCLF3,  SCLFl,  SCLF2)  are  all  associated  with  the  size 
small.  Those  determined  by  (PDl,  PD2,  PD3),  (DCPAl,  DCPA2,  DCPA3),  (SCLFl, 
SQJ^,  SCLF3)  are  associated  with  the  size  medium,  and  those  determined  by  (PDl, 
PD3.  PD2),  (DCPAl,  DCPA3,  DCPA2),  (SCLFl,  SCLF3,  SCLF2)  with  the  size  large. 

C.  CODE  CHANGES 

The  SAFOR  source  code  is  written  in  the  C  programming  language.  A  program 
written  in  C  consists  of  various  functions,  the  source  codes  of  which  may  be  collected 
together  in  arbitrarily  defined  groups  and  each  group  stored  in  a  separate  file.  The 
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corresponding  file  names  all  have  the  extension  c,  like  the  files  driver.c  and  avoid.c 
mentioned  in  Section  IV  A. 

As  observed  earlier,  the  source  code  of  the  function  avoid_a_vehicle,  which  is  the 
primary  fuix:tion  that  causes  all  vehicles  but  fixed  wing  aircraft  to  avoid  each  other,  appears 
in  the  file  avoitic.  The  source  code  for  the  version  of  avoid_a_vehicle  currently  used  in 
SAPOR  is  in  Appendix  C,  frixn  p.  C-3  to  p.  C-6.  The  source  code  for  the  fuzzy  versicxi  of 
avoid.a^vehicle  is  in  Appendix  C,  from  p.  C-10  to  p.  C-13. 

A  comparison  of  the  two  versions  of  avoid_a_vehicle  shows  that  the  only  effective 
difference  between  them  is  the  way  each  calculates  the  quantity  scale_factor.  The  fuzzy 
calculation  replaces  the  non-fuzzy  calculation 

"scalc_factor  = 

SGN(  dist_at_cpa )  ♦  AVOID_FACTOR  *  ( Passing_Dist  -  ( abs(  dist_at_cpa ) ) ) 

/  ( MAX(time_to_cpa,  MINIMUM_TIME) );" 
in  the  present  SAPOR  version  by 

"fsf = fuzzy_scale_factor(Passing_Dist,  dist_to_cpa,  speed); 

scale.factor  ~ 

SGN(  dist_accpa )  *  AVOID_FACrOR  *  fsf;". 

The  code  for  the  function  fuzzy_scale_factor  that  calculates  the  quantity  fsf  is  in 
Appendix  C,  from  p.  C-17  to  p.  C-24,  followed  by  code  for  functions  needed  to  support 
dte  calculaticm. 

After  viewing  a  video  tape  showing  preliminary  results  of  the  collision  avoidance 
experiment.  Dr.  Robert  Roberts  suggested  that  the  vehicle  behavior  would  be  more  realistic 
if  the  vehicles  slowed  down  before  engaging  in  their  avoidance  maneuvers.  Adding  this 
feature  turned  out  to  be  quite  elective,  causing  more  realistic  behavior  in  both  the  fuzzy 
and  non-fuzzy  versions  of  SAPOR.  In  fact,  when  the  standard  crisp  SAPOR  algorithm 
was  modified  to  include  slowing  down  it  became  in^ssible  to  make  the  vehicles  collide  at 
all. 
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Another  type  of  file,  called  a  header,  contains  definitions  and  has  the  extension  h.  Both  file  types  are 
compiled  into  object  files  with  the  extension  o,  which  are  then  linked  to  create  an  executable  file. 


Fortunately,  inctnporating  this  feature  in  either  avoidance  algorithm  is  a  simple 
matter.  If  the  scale.factor  is  less  than  1,  it  is  assumed  that  the  slow  down  is  unnecessary. 
Otherwise,  the  algorithm  divides  both  components  of  the  avoider's  current  velocity  vector 
by  the  scate  factor  before  adding  the  increment  required  for  the  avoidance  maneuver.  The 
easiest  way  to  see  how  this  was  done  is  to  compare  the  original  version  of  the  function 
avoid_a_vehiclc  in  Appendix  C,  pp.  C-3  to  C-6,  with  a  version  in  Appendix  C,  pp.  C-6  to 
C-8  that  includes  the  slow  down  feature.  The  fuzzy  version  of  avoid.a. vehicle  in 
Appendix  C,  p.  C-10  to  p.  C-13  can  also  be  compared  with  the  fuzzy  version  that  includes 
the  slow  down  feature,  and  which  can  be  found  in  Appendix  C,  p.  C-13  to  p.  C-17. 
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VI.  VIDEO  ACTION 


In  this  chapter  we  describe  a  video  that  shows  a  dramatic  result  obtained  from 
introducing  a  fuzzy  algorithm  into  SAFOR.  The  video  is  divided  into  two  parts.  The  first 
part  deals  with  SAFOR  and  fuzzy  logic  and  illustrates  some  unrealistic  vehicle  behavior  in 
SAFOR  exercises.  The  second  part  concentrates  on  vehicle  collisim  avoidance  problems. 

The  video  starts  with  a  scene  from  a  Hunter-Uggett  exercise  with  tanks  moving  in 
f(»mation  across  a  flat  terrain  and  over  hills.  Enemy  tanks  are  seen  moving  along  a  road. 
A  battle  ensues  and  the  enemy  tanks  are  destroyed  one  at  a  time.  As  they  go  up  in  flames 
the  last  tank  in  the  platoon  goes  around  the  burning  pile  and  continues  on  the  course 
eventually  to  be  destroyed  also.  Some  of  the  tanks  take  questionable  actions  in  the  scenes 
shown,  but  nothing  unusual  or  obviously  unrealistic  happens,  with  the  exception  of  a  tank 
running  through  a  tree,^^  until  the  tanks  approach  a  hilly  area  where  one  bumps  into  the 
one  ahead  of  it.  Soon  they  coalesce  into  a  single  tank.  A  shmt  time  later  a  third  tank  is 
seen  q)proaching  the  two  in  what  appears  to  be  a  "mating  dance."  The  three  combine  into  a 
single  "monster  vehicle"  with  three  turrets  independently  moving,  presenting  a  bizarre 
scene  to  the  viewer.  As  explained  in  foomote  1  on  page  1,  these  effects  are  not  due  to  any 
SAFOR  deficiency. 

The  second  and  third  scenarios  show  tanks  unsuccessfully  avoiding  a  lake  and  a 
building.  In  the  second  scene  one  tank  gets  into  the  water,  stops  and  rotates  aimlessly  in 
place.  The  following  tank  stc^s  a  few  meters  from  the  water  and  also  rotates  in  place.  In 
the  third  scene  a  tank  is  seen  approaching  a  building,  which  it  tries  to  avoid,  but  not,  as 
becomes  apparent  very  quickly,  very  successfully.  About  25  percent  of  the  tank  gets  into 
the  building  perhaps  because  the  avoidance  was  not  initiated  early  enough  or  the  turn  was 
not  sharp  enough.  Either  action  could  have  avoided  the  problem.  The  tank  gets  out  of  the 
building  and  makes  a  reasonable  atten^t  to  get  around  its  comer.  It  then  travels  parallel  to 
the  side  of  the  building  in  a  perfectly  reasonable  manner  until  suddenly,  about  half  of  the 
way  to  the  other  comer,  it  turns  and,  for  no  apparent  reason,  moves  right  through  the  wall. 


^  ^  There  is  no  provision  made  for  avoiding  isolated  trees  in  SAFOR,  cleariy  a  giveaway  in  an  exercise  but 
not  a  difficidt  problem  to  fix. 


These  three  scenes  were  included  in  the  video  to  point  out  some  obvious  SAPOR 
problems  not  involving  vehicle  collision  avoidance. 

Next  the  video  shows  a  vehicle  collision  course  depicted  in  Figure  9.  The  arrows 
indicate  the  direction  of  the  vehicle's  movement.  A  platoon  of  tanks  goes  along  the  road 
fn»n  the  top  to  the  bottom,  making  two  turns,  first  to  the  right  and  then  to  the  left.  An 
Amxned  Personnel  Carrier  (APC)  will  enter  from  the  left  and  move  along  a  road  toward 
the  intersection  where  the  tanks  make  the  left  turn. 

Although  the  scenario  and  troop  movements  were  taken  from  an  actual  military 
exercise,  the  dming  was  rearranged  to  test  the  SAPOR  collision  avoidance  algotidun.  This 
was  done  by  initializing  the  APC  movement  so  that  a  collision  would  occur  unless  the 
algorithm  prevented  it.  When  the  action  in  the  video  starts,  we  see  the  platoon  following 
the  road.  Figure  10(a)  shows  the  lead  tank  approaching  the  intersection  from  the  right. 
The  APC  at  this  time  is  out  of  the  picture  to  the  left  of  the  intersection.  The  leader  makes  a 
right  turn,  then  a  left,  and  then  gets  clobbered  by  the  APC  at  the  spot  shown  on  the  map  in 
Figure  l(Ka).  The  SAFOR  algorithm  did  not  work.  Figure  l(Xb)  shows  a  close  up  of  the 
collision. 

The  next  action  scene  starts  with  the  same  initial  conditions,  but  vrith  the  SAFOR 
code  modified  by  replacing  the  original  collision  algorithm  with  a  fuzzy  version.  This  time 
the  collision  is  avoided. 

A  closer  analysis  indicates  that  even  though  the  collision  was  avoided,  the  action 
did  not  seem  realistic.  For  example,  the  tanks  did  not  slow  down  during  the  avoidance 
maneuver  and  the  miss  distance  was  too  small.  Figure  l(Kc)  shows  the  APC  just  missing 
the  tank.  What  the  picture  does  not  reveal  is  that  both  vehicles  were  moving  rather  fast  at 
this  time.*2 

We  decided  to  supplement  collision  avoidance  with  slowdown.  We  inserted  a 
slowdown  operation  (dividing  the  vehicle  velocity  by  the  avoidance  scale  factor  whenever  it 
is  greater  than  1)  into  the  code  and  reran  the  action.  This  time  the  fuzzy  algorithm  did  much 
better.  Both  the  APC  and  the  tank  slowed  down  perceptively  and  the  tank,  after  making  the 


Some  viewers  commented  that,  realistically,  the  APC  would  stop  and  let  the  column  of  mnirs 
The  collision  avoidance  algorithm,  however,  does  not  discriminate  between  different  types  of  ground 
vehicles,  although  perhaps  this  could  be  changed. 
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Overhead  of  the  Area  of  Intereat  Wtth  Movement  of  the  Platoon 


FIgura  10(a).  Overhaad  View  of  the  Figure  10(b).  Collision  Between  Tank 

Lead  Tank  Approaching  the  inter*  and  APC  When  SAFOR 

section.  This  scenario  Is  the  initial  Algorithm  Is  Used 

condition  for  all  the  runs  shown  here. 


Figure  10(c).  Avoidance  of  Collision  Figure  10(d).  Avoidance  of  Collision 
When  Fuzzy  Algorithm  Is  Used  When  Fuzzy  and  Slowdown 

Algorithms  Are  Used 


left  tuni,  actually  stopped  and  let  the  APC  pass.^^  Figure  10(d)  shows  an  overhead  view 
with  die  tank  actually  stopped  and  the  APC  avoiding  it  about  a  tank's  length. 

We  also  tried  the  slowdown  with  the  original  crisp  SAFOR  algorithm.  The 
collision  was  avoided  but  again  the  avoidance  was  just  barely  successful  and  the  vehicles 
went  past  each  odier  faster  than  reasonable. 


Whether  a  real  tank  would  actually  stop  to  let  an  APC  pass  is  questionable.  Further  refinements  may 
be  necessary. 
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VEHICLE  PATH  WHILE  AVOIDING  OBSTACLES 


Figure  2  Crisp  Algorithm  Avoidance  of  Spherical  Obstacles  Set  2 


Figure  4  Ciisp  Algorithm  Avoidance  of  Spherical  Obstacles  Set  4 


Figure  5  Crisp  Algorithm  Avoidance  of  Spherical  Obstacles  Set  5 


Figure  6  Crisp  Algorithm  Avoidance  of  Cylindrical  Obstacles  Set  1 
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Flsure  7  Crisp  Algorithm  Avoidance  of  Cylindrical  Obstacles  Set  2 


Figure  8  Crisp  Algorithm  Avoidance  of  Cylindrical  Obstacles  Set  3 


Figure  9  Crisp  Algorithm  Avoidance  of  Cylindrical  Obstacles  Set  4 
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Figure  10  Crisp  Algorithm  Avoidance  of  Cylindrical  Obstacles  Set  5 
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APPENDIX  B 

CREATING,  STORING,  AND  REPLAYING 
SAPOR  EXERCISES  ON  SIMNET 


1 .  SILICON  GRAPHICS  WORK  STATIONS 

a.  Introduction 

w.  eating  a  SAPOR  scenario  or  playing  back  one  that  has  been  saved  in  a  file 
requires  two  Silicon  Graphics  work  stations,  one  called  the  front  end  and  the  other  the  back 
end.  Logging  a  scenario,  i.e.,  saving  it  for  future  playback,  requires  a  third.  However, 
without  interfering  with  any  other  user  that  it  may  have,  the  third  work  station  can  be 
accessed  via  the  netwoiic  and  displayed  on  the  back  end  terminal. 

A  logged  scenario  saved  in  a  rile  can  not  only  be  played  back  using  two  Silicon 
Graphics  work  stations,  it  can  also  be  run  on  the  Stealth  system.  VCR  equipment  in  the 
Stealth  room  can  record  the  scenario,  or  selected  portions  of  it  viewed  in  any  of  the 
available  Stealth  modes,  on  videott^. 

Since  the  Silicon  Graphics  operating  system  is  Unix  an  elementary  knowledge  of 
Unix  is  occasionally  helpful  in  carrying  out  the  steps,  e.g.,  for  deleting  or  changing  the 
name  of  a  rile.  A  paperback  text  on  the  subject  is  sometimes  available  in  the  SIMNET  area. 

The  next  two  sections  detail  the  basic  steps  needed  to  create,  record,  and  play  back 
SAPOR  scenarios.  The  steps,  which  are  in  the  necessary  order  of  their  application,  are 
numbered  for  easy  reference.  Italicized  wonls  or  phrases  in  the  text  are  commands  or 
labels  as  they  actually  appear  (Hi  a  Silicon  Graphics  terminal  screen. 

b.  Steps  for  Creating  a  Scenario 

(1)  Set  up  two  Silicon  Graphics  terminals:  alex  and  Springfield,  making  alex  the 
back  end  and  Springfield  the  front  end.  Start  by  logging  in  on  each  terminal  and  then  going 
to  the  appropriate  SAPOR  subdirectory,  which  is  the  same  for  both,  by  entering  the 
command: 


cd  lusrlst^4J3.  On  alex  set  up  the  back  end  connection  with  the  terrain  data  base* 
and  assign  an  exercise  number  by  entering  the  command:  phantom  -terrain  hunter-0110  -e 
18.  To  set  up  the  front  end  on  Springfield  use  the  command: 

ws  -terrain  hunter-0110  -simjex  18  -po_ex  18.  As  indicated,  the  exercise  is  labeled 
Id,  which  is  arbitrary  but  chosen  here  because  it  is  unlikely  to  be  the  same  as  the  number 
associated  with  a  Stealth  exercise  that  may  be  going  on  at  the  same  time,  since  the  default 
number  is  1.  After  a  short  wait  a  map  will  app)ear  on  the  Springfield  screen. 

(2)  Use  die  following  procedure  to  connect  the  front  end  with  the  back  end.  At  the 
top  of  the  screen  click  on  Fi7e~then,  in  the  menu  that  drops  down,  on  Connect-xhcn,  in  the 
new  menu  appearing  on  the  right,  on  alex.  A  message  at  the  top  of  the  screen  will 
announce  the  connection  when  it  occurs,  or  a  failure  if,  for  some  reason  it  does  not. 

(3)  Select  the  region  of  interest  on  the  map  and  enlarge  its  image  on  the  screen.  To 
do  this  move  the  cursor  to  the  center  of  the  region  and  press  the  middle  mouse  button  to 
zo(Mn  in.  By  dragging  the  number  in  the  panel  labelled  Vehicle  Scale  at  the  bottom  of  the 
screen  mi  the  ri^t,  at  any  time  the  mouse  can  be  used  to  set  a  scale  that  determines  the  size 
of  the  icons  representing  vehicles  on  the  map.  Hie  number  increases  when  dragged  to  the 
right  and  decreases  when  dragged  to  the  left. 

(4)  Create  an  Overlay  by  first  clicking  on  Overlay  at  the  top  of  the  screen  and  then 
on  Create  in  the  resulting  menu  that  drops  down.  A  panel  will  appear  at  the  bottom  of  the 
screen  requesting  an  Overlay  name.  Enter  a  name  in  the  designated  area;  then  click  cm  OK. 

(5)  To  add  vehicles  to  the  Overlay  click  mi  the  tank  shaped  icon  on  the  right  side  of 
the  screen.  A  menu  will  appear  requesting  the  echelon  level.  Click  on  the  one  desired,  and 
drag  the  mouse  to  the  left  Then  a  menu  with  a  list  of  vehicle  types  will  drop  down.  After 
clicking  on  one  of  those  listed,  a  military  symbol  of  the  choice  will  appear  on  the  map.  To 
change  the  symbol  to  one  more  pictoiially  representative  of  the  actual  vehicle,  click  on  the 
designation  Non-Military  among  the  items  listed  on  the  bottom  left  of  the  screen.  Other 
items  listed  in  the  same  area,  such  as  Contour  Lines,  which  displays  constant  terrain 
altitude  curves  mi  the  map,  may  also  be  useful. 


*  All  scenarios  discussed  in  this  document  take  place  on  terrain  defined  by  data  stored  in  the  Hunter- 
Liggett  data  base. 
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(6)  To  edit  or  get  information  about  an  Overlay  vehicle  click  (md  the  icon  in  eidier 
the  military  or  nonmilitary  representation.  This  will  produce  a  small  menu  including  the 
optitm  Describe,  which,  when  selected,  provides  the  vehicle  type  and  LD.  number. 

Another  opticm  is  Edit,  which  can  be  used  to  make  certain  changes,  including  the 
vehicle's  location.  Clicking  on  Edit  produces  a  panel  containing  options  such  as 
Alignment,  which  allows  the  user  to  select  a  Friendly  (US)  or  Enemy  (Soviet)  type  of 
vehicle,  or  Echelon,  which  allows  the  user  to  change  to  any  echelon  in  the  range  from  a 
single  vehicle  to  a  battalion.  Among  the  options  included  is  one  labelled  Direction  (mils) 
with  an  associated  number  in  a  nanow  horizontal  track.  Clicking  on  the  number,  which  is 
at  the  left  end  of  die  track  and  is  0  initially,  and  dragging  it  to  the  right  has  two  effects.  It 
causes  the  vehicle  to  turn  in  the  clockwise  direction  and  the  number  to  increase.  Dragging 
to  the  left  reverses  both  effects.  Also,  while  in  the  edit  mode,  clicking  anywhere  on  the 
map  will  cause  the  vehicle  to  move  to  the  cursor  position.  To  leave  the  edit  mode  click  tm 
OK. 

(7)  To  use  the  overlay  vehicles  in  an  exercise  it  is  necessary  to  simulate  them  by 
clicking  on  Simulate  in  the  Overlay  menu.  A  military  symbol  will  replace  each  vehicle  icon 
on  the  map.  To  return  to  the  nonmilitary  form  click  on  Show  As  located  at  the  top  of  the 
screen  on  die  left-then  on  Vehicle  in  die  resulting  menu  that  appears. 

Although  both  are  initially  at  the  same  location,  the  simulated  vehicle  has  an  identity 
separate  from  the  one  originally  created  in  the  Overlay.  Clicking  on  the  simulated  vehicle 
produces  a  menu  that  includes  the  c^ticms:  Describe,  Edit,  TACIE,  and  Unit  Tasking. 

The  first  two  items  perform  the  same  functions  for  the  simulated  vehicle  as  those  in 
the  menu  associated  with  the  original  Overlay  vehicle.  However,  the  changes  that  can  be 
made  in  the  panel  created  by  clicking  on  Edit  are  now  limited  to  changing  the  orientaticxi  of 
the  simulated  vehicle  and  moving  it  to  another  location. 

(8)  To  get  the  vehicles  moving  in  the  exercise  use  either  the  TAOE  or  the  Unit 
Tasking  cation.  Clicking  on  TAOE  produces  a  menu  containing  several  vehicle  override 
and  command  options. 

Important  override  settings  are  Speed,  which  allows  values  ranging  from  0  to  8S 
km/hr  and  Formation.  Clicking  on  formation  and  then  on  new  formation  in  the  panel  that 
appears  leads  to  a  menu  on  the  left  containing  from  5  to  10  options,  depending  on  the 
echeltxi  level 


Several  useful  ccmimands  under  TACIE  are  Go  To  Location,  Go  To  Location  Via 
Road,  and  Follow  Route.  Clicking  on  either  Go  To  Location  option  causes  the  appearance 
of  a  panel  that  is  best  ignored.  Instead,  just  click  on  the  desired  location  and  on  OK  in  the 
panel;  the  vehicle  will  then  move,  directly  in  the  first  case  and  in  a  roundabout  way  in  the 
seoNid,  to  the  indicated  spot  To  accomplish  the  same  end  by  having  the  vehicle  follow  a 
prescribed  route,  use  Unit  Tasking. 

(9)  To  create  a  route,  which  is  necessary  before  a  vehicle  can  follow  it  click  (xi  the 
route  icoi  on  the  right  side  of  the  screen.  Before  doing  so,  however,  it  is  advisable  to  click 
<Mi  Overlay  at  the  top  of  the  screen  and  then  on  Show.  In  the  resulting  panel  make  sure 
that  along  with  the  button  labeled  Simulation  the  one  labeled  Overlay  is  depressed; 
otherwise,  the  route  path  will  not  be  visible.  Then  click  on  the  starting  point  of  the  route  on 
the  mi^.  Qick  on  the  next  route  point  connected  by  a  straight  line  with  the  first.  Continue 
to  add  straight  line  segments  to  the  route  by  clicking  on  the  locations  of  their  endpoints  until 
the  route  is  completed.  Note  that  in  the  accompanying  panel  a  choice  between  solid  or 
dashed  line  segments  is  available.  After  completing  the  route  definition  click  on  OK  in  the 
panel. 

(10)  There  are  two  ways  to  command  a  vehicle,  or  a  higher  echelon  unit  of 
vehicles,  to  follow  the  route.  One  is  to  use  the  Follow  Route  option  under  TACIE  in  the 
menu  obtained  by  clicking  on  the  vehicle  or  unit  icon.  The  other  is  to  invoke  Unit  Tasking 
rather  than  TACIE.  The  second  method  produces  a  panel  that  requires  confirmation  of  the 
Overlay  name,  the  route  name,  and  the  CIS^,  which  for  route  following  expects  the 
Roadmarch  option.  Click  on  each  item,  making  sure  that  each  corresponding  name  appears 
under  the  appropriate  heading  in  the  sub-pane!  below.  When  this  happens  a  CIS  editor 
panel  appears.  Select  the  vehicle  speed  and  click  on  OK.  The  Unit  Tasking  panel  will  then 
re^>pear.  To  start  the  vehicle  on  its  way  click  on  Execute  at  the  bottom  line;  then  click  on 
OK  to  remove  the  panel. 

(11)  To  save  an  overlay,  which  is  useful  for  modifying  a  scenario  after  running 
one  version  without  having  to  go  through  all  of  the  preliminary  steps  again,  just  click  on 
Overlay  and  then  on  Save  in  the  ensuing  menu.  A  panel  requesting  a  file  name  will  appear. 
Enter  a  file  name  and  click  on  OK. 

(12)  To  start  over  using  the  same  Overlay,  including  vehicles  and  routes,  click 
on  File,  then  on  Delete  All  in  the  menu  that  drops  down.  After  confirming  the  command  by 
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clicking  on  OK  in  the  ensuing  panel,  click  on  Overlay,  followed  by  Load  in  the  resulting 
menu  that  iq>pears.  A  menu  containing  the  list  of  saved  Overlay  files  will  then  appear, 
qicking  on  the  filename  of  a  previously  saved  Overlay  will  recreate  it.  N.B.:  to  continue 
it  is  necessary  to  simulate  the  appropriate  Overlay  entities  by  clicking  on  Simulate  in  the 
Overlay  menu,  as  well  as  the  correct  Overlay  name  appearing  on  the  right 

c.  Some  Useful  Aids  in  Designing  and  Viewing  a  Scenario 

nicking  on  the  middle  mouse  buttcMi  and  dragging  creates  a  rectangular  frame,  the 
upper  left  hand  comer  of  which  starts  at  the  initial  cursor  position.  The  lower  right  hand 
comer  follows  the  cursor.  When  the  mouse  button  is  released  the  region  inside  the  frame 
beccHnes  die  center  of  the  screen  view,  occupying  a  larger  fraction  of  the  screen  than  it  did 
before.  The  smaller  the  frame  the  more  the  scene  within  it  will  be  magnified  in  this 
process. 

To  change  the  time  between  updates  of  the  nnoving  vehicle  positions  click  on 
Options  at  the  top  of  the  screen-then  click  on  Pvd  Display  Options  in  the  menu  that  drops 
down.  A  panel  vnll  appear  at  the  bottom  of  the  screen  containing  a  track  labeled  Seconds 
Between  Update.  The  number  5  appears  a  short  distance  frrom  the  track's  left  end. 
Qicking  on  the  number  and  dragging  to  the  left  reduces  it  and,  therefore,  the  time  between 
updates.  This  makes  the  vehicle  motion  less  jeriry.  Dragging  as  far  as  possible  to  the  left 
reduces  the  number  to  0  and  provides  the  smoothest  possible  motion. 

Setting  a  marker  at  a  point  on  a  route  can  be  a  useful  device  in  synchronizing  the 
nootitHi  of  two  different  vehicles,  e.g.,  delaying  the  start  of  one  until  the  instant  the  other 
passes  the  marker.  One  way  to  mark  a  point  without  interfering  with  the  scenario  is  to 
create  another  Overlay  and  then  from  annong  the  entities  available  in  the  vehicle  nrenu, 
choose  the  dismounted  infantry.  After  clicking  on  the  corresponding  icon,  which  should 
be  set  to  the  nonmilitary  version  as  described  in  Step  5,  use  the  Edit  option  in  the  resulting 
menu  that  appears,  as  described  in  Step  6,  to  move  it  to  the  point  where  the  marker  is 
needed.  Do  not  simulate  the  Overlay  in  which  the  dismounted  infantry  used  for  a  marker 
was  created.  To  be  able  to  repeat  the  initial  scene,  save  that  Overlay  and  the  Overlay 
containing  the  vehicles  and  routes  in  separate  files. 

When  redoing  a  scenario  by  restoring  its  initial  state,  restore  each  Overlay 
separately  by  clicking  on  Load  in  the  Overlay  menu  and  selecting  its  frlename  in  the 
resulting  list  that  appears.  Simulate  the  Overlay  containing  the  vehicle  entities  and  the 
routes,  but  do  not  simulate  the  dismounted  infantry  Overlay. 


d.  Steps  in  Saving  a  Scenario 

(1)  The  process  begins  on  roanolce,  which  can  access  the  required  logging 
piognun.  It  is  not  necessary  to  log  in  on  the  loanoke  terminal.  Instead,  while  logged  in  on 
alex  create  a  new  window  and  enter  the  command  telnet  roanoke.  At  the  resulting  prompt 
log  in  as  usual.  Then  enter  the  command  cd  Iusrls(rfl433  followed  by  Otblgr  -display 
alex3).0,  which  allows  the  display  of  the  logger  control  panel  to  appear  on  the  alex  terminal 
screen. 

(2)  After  the  logger  control  panel  appears  type  in  the  name  of  the  Hie  in  which  the 
scenario  is  to  be  stored.  The  filename  should  include  a  path  starting  with  lusripeoplel 
followed  by  a  subdirectcny  in  which  the  user  has  write  privileges  on  roanoke.  Press  the 
Return  key;  type  in  the  exercise  number,  i.e.,  18,  in  the  designated  place,  and  press  the 
Return  key. 

(3)  Set  up  alex  and  Springfield  as  in  step  b(l)  and  connect  them  as  in  step  b(2),  if 
the  setup  and  connection  do  not  already  exist.  On  alex  this  can  be  done  by  bringing  up 
another  window  for  the  purpose.  Once  the  connection  is  made  switch  back,  making  the 
roanoke  window  containing  the  logger  control  panel  active. 

(4)  On  Springfield  create  and  run  the  scenario  to  be  recorded  as  in  steps 
b(3>-b(l  1).  Click  on  Record  at  the  top  of  the  logger  control  panel  on  the  alex  terminal  to 
Stan  the  recording  session.  This  can  be  done  eidier  before  or  after  the  scenario  is  started  on 
Springfield.  To  end  the  recording  session,  click  on  Stop  at  the  top  of  the  logger  control 
panel  on  alex. 

(5)  To  play  back  a  recorded  scenario  Springfield  should  not  be  connected  to  alex, 
but  the  logger  control  panel  cm  roanoke  must  be  created  and  displayed  either  on  roanoke  or 
cm  alex.  To  create  and  display  it  on  alex  follow  the  step  c(l)  procedure.  Otherwise,  log  in 
on  roanoke,  type  the  command  cd  lusrlsc^433,  press  the  Return  key,  and  then  type  the 
command  Otblgr.  The  logger  control  panel  will  then  appear  cm  roanoke. 

In  either  case,  in  the  designated  place  enter  the  file  name,  including  the  correct  path, 
which  starts  out  with  lusripeoplel.  Press  the  Return  key.  The  exercise  number  18  used  for 
the  recording  session  should  appear  in  the  place  reserved  for  it  on  the  panel,  and  a  number 
^ould  2q)pear  in  Time  Remaining  indicating  the  amount  of  time  the  scenario  will  run.  The 
playback  can  be  speeded  up  or  slowed  down  by  clicking  on  the  speed  factor  track  labelled 
LOxSpeed  and  dragging  it  to  the  right  or  left.  The  number  replacing  1.0  gets  smaller  while 
dragging  to  the  left  and  larger  while  dragging  to  the  right 
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To  start  the  scenario  click  on  Play  or  Loop  Play  at  the  top  of  the  panel.  To  su^  the 
play  back  and  begin  again,  click  on  5rpp~then  click  on  and  drag  the  button  in  the  Time 
trade  as  for  as  it  will  go  to  the  left-thoi  click  on  Play  or  Loop  Play  once  more. 

2.  PLAYING  BACK  AN  EXERCISE  ON  STEALTH 

The  first  step  requites  that  an  authorized  person  log  in  as  root  on  the  right  hand 
terminal.  Once  this  is  done,  enter  the  same  commands  as  those  used  on  the  Silicon 
Gnq>hics  tetminal  Springfield  in  step  b(l)  except  that  4.3.3  must  be  replaced  by  4.3.6  and 
yws  must  be  used  in  place  of  ws.  The  same  map  that  would  appear  on  Springfield  will 
iqppear  on  the  right  hand  Stealth  terminaL 

Next,  on  the  left  hand  terminal  follow  the  same  procedure  as  in  d(l),  first  entering 
telnet  roanoke,  then  logging  in  (not  necessarily  as  root)  and  entering  cd  lusrls(rfl4JJ. 
Then  enter  Otblgr  -display  idaobgl:0.1  <&,  which  is  similar  to,  but  not  the  same  as,  the 
command  in  d(l).  The  logger  control  panel  will  appear  on  the  left  hand  termiiud  just  as  it 
does  <Mr  alex  in  the  procedure  of  d(l). 

Follow  the  procedure  described  in  d(2)  to  play  back  a  stored  exercise,  but  before 
playing  it  go  to  the  right  hand  terminal.  Under  Options  at  the  top  of  the  screen  click  on  the 
last  item,  which  has  the  form  FC(nin2)  with  numbers  ni  and  n2  that  may  vary,  and  taxer 
the  conect  exercise  number,  le.,  18,  in  the  Flying  Carpet  panel  that  appears. 

Clicking  on  the  tetminal  screen  causes  a  white  arrow  that  locates  the  viewing 
position  cm  the  displayed  miq)  and  points  in  the  viewing  direction  to  jump  to  the  cursor 
position.  At  the  same  time  the  observer's  stealth  viewpoint  nooves  to  the  new  location 
indicated  the  arrow  on  die  miq). 

Other  options  on  the  Flying  Carpet  panel  are  dynamic  viewpoints,  which  include 
attaching  cn*  tethering  to  a  vehicle.  To  select  one  click  on  the  panel  and  then  on  the  desired 
vehicle. 

The  observer’s  viewpoint  can  also  be  controlled  by  the  spaceball  to  the  right  of  the 
right  hand  tenminal.  The  ball  can  change  the  viewpoint  l(x;ation  in  three  dimensiems  and  its 
numerical  altitude  relative  to  the  ground  is  displayed  in  a  panel  on  the  tominal  screen.  A 
line  of  buttems  numbered  from  1  to  8  at  the  front  end  of  the  spaceball  base  control  the 
Flying  Carpet  view  as  indicated  in  Table  B-1. 


Tabl«  B-1.  Spac«tMill  Quick  Rdferenc*  Flying  Carpet 


Constant  Altitude 

Rotation  On/Off 

Dominant  Mode 

Level  View 

1 

2 

3 

4 

Free  Fly 

Increase  Sensitivity 

Decrease  Sensitivity 

Re'Zero 

5 

6 

7 

8 
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APPENDIX  C 

SAPOR  VEHICLE  COLLISION  AVOIDANCE  CODE 


1.  Present  Cri^  Version 


y*«*4i«**4i4i*******«**4i**4i*4i4i*4i**4i*****«******4i*****4>**************4i*4i*4i**4>****4i 


Modify  the  desired  velocity  of  the  avoiding  vehicle 

*  a  quantity  apprcyriate  to  making  a  greater  gap  between  the  two 

*  vehicles  at  their  projected  cpa  (closest  point  of  approach) 


0*tHit*4>**********************************************************************/ 


void  avoid_ajvdiicle(  avoider,  avoidee,  desired.vel,  ^al_pt ) 


SAF.OBJECT  ♦avoider,  *avoidee; 

VECTOR  desired_vel,  goal_pt; 

{ 

VECTOR  rel_pos,  reLvel,  delta_vel,  delta_pos,  rel_pos_of_point; 
double  dist_at_cpa,  dist_to_cpa,  titne.to.cpa,  rel.speed,  scale.factor; 
double  tiine_to_point,  dist_to_point,  q)eed; 

double  time_stagger  =  ( avoider->tickable->now  -  avoidee->tickable->now ) 
*.001; 

void  increnient_desired_velO; 


/*  calculate  relative  position  taking  into  ctmdderaticm  the  difference 


VEC2_SIJB(  avoidee->cntity->position,  avoider->cntity->position,  rel_pos ); 
VEC2_MULT(  dme.stagger,  avoidee->entity->vclocity,  delta_pos ); 
VEC2_ADD(  delta_pos,  rel_pos,  rel_pos ); 

f*  calculate  distance  at  closest  point  of  approach  */ 

VEC2_SUB(  avoider->cntity->vclocity,  av<Mdec->cntity->velocity,  rcLvel ); 
leLspeed  =  vec2_inag(  rel_vcl ); 
if(  leLspeed  <  SMALL ) 
return; 

dist_at_cpa  =  VEC2_CROSS(  rel_vel,  rel_pos )  /  rel_specd; 

f*  if  this  distance  is  greater  than  a  threshold,  no  need  to  avoid  */ 
if(  abs(  dist_at_cpa )  >  Passing_Dist ) 
return; 

/*  calculate  time  to  closest  point  of  approach  */ 
dist_to_cpa  =  VEC2JDOT(  rel_vel,  rel_pos )  /  reLspeed; 
if(  dist_to_cpa  <=  0 ) 
return; 

tinie_to_cpa  =  dist_to_cpa  /  reLspeed; 
if(  tiiiie_to_cpa  >  GRND_LOOK_AHEAD ) 


return; 


/"  if  diere  is  a  goal  point,  calculate  time  to  it  */ 
if(  goaLpt !«  Null_VectcM‘ )  { 
speed  s  vec2_inag(  avoider->entity->velocity ); 
if(  speed  <  SMALL) 
return; 

VEC2_SUB(  goal^t,  avoidcr->cntity->position,  rel_pos_of_point ); 
dist_to_point  *  vec2_mag(  rel_pos_of_point ); 
tinie_tD_point  =  dist_tD_point  /  speed; 

/*  if  will  readi  goal  ptwt,  before  collision,  don't  do  avoidance  */ 
if(  time.tOLpoint  <  time.to.cpa ) 
return; 

} 

/*  calculate  magnitude  in  change  of  velocity  in  directicMi  normal  to 
rdadve  velocity  away  frcxn  collision  */ 
scale.factor  s 

SGN(  dist_at_cpa )  *  AVOID_FACTOR  ♦  ( Passing_Dist  -  ( abs(  dist_at_cpa ) ) ) 
/  ( MAX(time_K)_cpa,  MINIMUM_TIME) ); 


ddttL.vel[  0  ]  =  icLvclI  1  ]  ♦  scalc_factorA«l_specd; 
deltajveU  1  ]  =  -  rel_vcl[  0  ]  *  scalc_factorA«l_specd; 


VEC2_ADD(  desired_vel,  dclta_vcl,  dcsired.vel ); 

) 


f*******m********************************************************************/ 


SAF_OBJECT  ♦avoidcr,  *avoidcc; 

VECTOR  dcsired_vcl,  goal j>t; 

{ 

VECTOR  rel_pos,  rel_vcl,  delta_vcl,  delta_pos,  irl_pos_of_point,old_vcl; 
double  dist_at_q)a,  dist_to_q)a,  tiinc_to_q)a,  rel_specd,  scale_factor, 
double  tiine_to_point,  dist_to_point,  speed,  scf; 

double  tinMLStagger  =  ( avoider->tickable->now  -  avoidee->tickable->now ) 
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void  increment.desircd_vel(); 


VEC2_COPY(avoider->cntity->vclociiy,old_vel); 

t*  calculate  rdadve  position  taking  into  consideration  the  difference 
in  tick  times  */ 

VEC2_SUB(  avoidee->entity->position,  avoidcr->cntity->position,  rcl_pos ); 
VEC2_MULT(  time.stagger,  avoidce->cntity->vclocity,  deita_pos ); 
VEC2_ADD(  delta_pos,  rcLpos,  rel_pos ); 

I*  calculate  distance  at  closest  point  of  approach  */ 

VEC2_SUB(  avoider->entity->velocity,  avddcc->cntity->vclocity,  rcLvcl ); 
reL^wed  =  vec2_inag(  rel_vcl ); 
if(  leLspeed  <  SMALL ) 
return; 

dist_at_cpa  *  VEC2_CR0SS(  rel_vel,  rel_pos )  /  reLspeed; 

I*  if  this  distance  is  greater  than  a  tiireshold,  no  need  to  avoid  */ 
if(  abs(  dist_at_cpa )  >  Passing_Dist ) 


return; 


/*  rime  to  closcst  point  of  approach  */ 

dist_to_cpa  =  VEC2_DOT(  rel_vcl,  rel_pos )  /  rcLspecd; 

I*  if  avoider  is  already  past  avoidee  then  return  to  desired  velocity  and 
quit*/ 

if(  dist_to_cpa  <=  0 ) 

{ 

if(!aS_AIRCRAFT(OBJ_OBJECT_TYPE(avoidcr)))) 

{ 

avoidcr->entity->vclocity[0]  =  old_vcl[0]; 
avoider->cntity->velocity[l]  =old_vcl[l]; 

} 

return; 

} 

titne_to_cpa  =  dist_to_cpa  /  rcLspeed; 
if(  tiiiie_to_cpa  >  GRND_LOOK_AHEAD  ) 
return; 


/*  if  there  is  a  goal  point,  calculate  time  to  it  */ 
if(  goal_pt  !=  NulLVector )  { 
speed  =  vec2_mag(  avoider->entity->velocity ); 
if(  speed  <  SMALL) 
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return; 


VEC2_SUB(  goal_pt,  avoidcr->cntity->position,  rel_pos_of_point ); 
dist_to_point  =  vec2_inag(  reLpos_of_point ); 
tiine_to jwint «  dist_to_point  /  speed; 

!*  if  will  reach  goal  p<nnt,  before  collision,  don't  do  avoidance  *! 
if(  tiine_to_point  <  tiine_tOL.cpa  ) 
return; 

} 

/*  calculate  magnitude  in  change  of  velocity  in  direction  normal  to 
relative  velocity  away  from  collision  *! 
scf  * 

AVOID_FACTOR  *  ( Passing_Dist  -  ( abs(  dist_at_cpa ) ) ) 

/  ( MAX(time_to_cpa,  MINIMUM_TIME) ); 

/*  if  the  scale  factor  is  not  too  small  slow  down  before  incrementing  the  velocity  *! 
if(!(IS_AIRC3iAFT(OBJ_OBJECT_TYPE(avoider)))) 

( 

if(scf  >  1.0)vec2_div(scf4ivoidcr->cntity->velocity,  desired_vel); 

) 


scale.factor  =  SGN(dist_at_cpa)*scf; 

deita_vcl[  0  ]  =  rel_vcl[  1  ]  ♦  scale.factwAeLspeed; 


ddta^vcl[  1  ]  =  -  tcl_vcl[  0  ]  *  scale_factor/tel_speed; 
VEC2_ADD(  desired_vel,  delta_vcl,  desired_vel ); 


) 


^mtiitti$*0^i*m4t4ni**^iiii*0**$i*intm**************************************************^ 


3.  Fuzzy  Version 

^ititt^ftt^ttittiiiti/^ttttiti*************************************************************^ 

*  Modify  the  desired  velocity  of  the  avoiding  vehicle  by 

*  a  quantity  appropriate  to  making  a  greater  gap  between  the  two 

*  vehicles  at  their  projected  cpa  (closest  point  of  approach) 

void  avoid_a_vehiclc(  avoider,  avoidee,  dcsired_vcl,  goal_pt ) 


SAF_OBJECT  *avoider,  *avoidec; 

VECTOR  desired_vel,  goal_pt; 

{ 

float  fiiz^.scale.factorO; 

VECTOR  reLpos,  reLvel,  delta_vel,  delta_pos,  rel_pos_of_point; 
double  dist_at_cpa,  dist_to_cpa,  tiinc_to_cpa,  reLspeed,  scale_factor, 
double  time_to_point,  dist_to_4X)int,  speed; 

double  dme.stagger  =  (  avoider->tickable->now  -  avoidee->tickable->now  ) 

♦.001; 
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float  fsf; 


void  incremenL.desiied_vel(); 

/*  calculate  relative  positicMi  taking  into  consideradoi  the  difference 
in  tick  times  */ 

VEC2_SUB(  avoidee->entity->position,  avoider>>entity->position.  rel_pos ); 
VEC2_MULT(  tiine.stagger,  avoidee->cntity->vdocity,  delta_pos ); 
VEC2_ADD(  delta_pos,  rel_pos,  rel_pos ); 

/*  calculate  distance  at  closest  point  of  approach  */ 

VEC2_SUB(  avoider->entity->velocity,  avddee->entity->velocity,  reLvcl ); 
reLspeed = vec2_mag(  reLvcl ); 
if(reLspccd<  SMALL) 
return; 

dist_at_cpa  =  VEC2_CROSS(  rel_vel,  rel_pos )  /  reLspeed; 

/*  if  this  distance  is  greater  than  a  threshold,  no  need  to  avoid  V 
if(  abs(  dist_at_cpa )  >  Passing_Dist ) 
return; 


/*  calculate  time  to  closest  point  of  approach  */ 
dist_to_cpa  =  VEC2_DOT(  reLvcl,  reLpos ) '  eLspced; 


if(  dist_to_q)a  <=  0 ) 
return; 

time.to jcpa  »  dist^to jcpa  /  reLspeed; 

/*  if  diere  is  a  goal  point,  calculate  time  to  it  */ 
if(  goaLpt  !=  Null_VectOT )  { 
speed  =  vec2_mag(  avoider->entity->vclocity ); 
if(  speed  <  SMALL) 
return; 

VEC2_SUB(  goal^t,  avoider->entity->position,  rel_pos_of_point ); 
dist_to_point  =  vec2_inag(  rel_pos_of_point ); 
time_to_point  =  dist_to_p(nnt  /  speed; 

/*  if  will  reach  goal  point,  before  collision,  don't  do  avoidance  *1 
if(  time_to_point  <  tiiiie_tOLcpa ) 
return; 

} 

/*  calculate  magnitude  in  change  of  velocity  in  directicm  normal  to 
relative  velocity  away  fixnn  collision  */ 
fsf  *  fuz2y_scalc_factor(Passing_Dist,  dist_to_cpa,  speed); 
scale.factor  = 
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SGN(  disuaucpa )  ♦  A  VOID_FACTOR  *  fsf; 
delta^vel[  0  ]  =  reLvcl[  1  ]  *  scalc_factor/reLspecd; 
delta^vel[  1  ]  *  -  rel_vcl[  0  ]  *  scale.factor/reLspeed; 
VEC2_ADD(  desiied.vel,  dclta_vcl,  desircd_vcl ); 

} 


^:tt**********************ti***‘************************************************f 


4.  Fuzzy  Version  with  Slow  Down 

^******1********************************************************************^ 

*  Modify  the  desired  velocity  of  the  avoiding  vehicle  by 

*  a  quanti^  impropriate  to  making  a  greater  gap  between  the  two 

*  vehicles  at  their  projected  cpa  (closest  point  of  approach) 

void  avoid_a_vehicle(  avoider,  avoidee,  desiied.vel,  goal_pt ) 


SAF_OBJECT  ♦avoider,  *avoidee; 

VECTOR  desired.vel,  goal_pt; 

{ 

float  fuz^_scale_factorO; 

VECJTOR  rel_pos,  reLvel,  delta_vcl,  delta_pos,  rel_pos_of_point,old_vel; 
double  dist_at_cpa,  dist_to_cpa,  time_to_cpa,  rel_speed,  scale.factor, 
double  time_u>_point,  dist_to_point,  speed; 
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double  dme.stagger  =  ( avoider->dckable->now  >  avoklee->tickaUe->now ) 


*.001; 

float  fsf; 

void  increiiient;_desired_vel(); 

VEC2_CX)PY  (av<nder->entity->velocity  ,old_vel); 

/*  calculate  relative  posititm  taking  into  consideration  the  difference 
in  tick  times  */ 

VEC2_SUB(  avoidee->entity->position,  avoider>>entity->position,  rel_pos ); 
VEC2_MULT(  time.stagger,  avoidee->entity->velocity,  delta_pos ); 
VEC2_ADD(  delta_pos,  rel_pos,  rel  jx)s ); 

/*  calculate  distance  at  closest  point  of  approach  */ 

VEC2_SUB(  avoider->entity->velocity,  avoidec-‘>cntity->vclocity,  rel_vel ); 
reLspeed  =  vcc2_inag(  rel_vel ); 
if(  reLspeed  <  SMALL ) 
return; 

dist_at_cpa  »  VEC2_CROSS(  rel_vel,  rel_pos )  /  reLspeed; 

/*  if  this  distance  is  greater  than  a  threshold,  no  need  to  avoid  */ 
if(  abs(  dist_at_cpa )  >  Passing_Dist ) 
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return; 


I*  calculate  time  to  closest  point  of  iqjproach  *! 
dist_to.cpa  =  VEC2_D0T(  rel_vel,  rel_pos )  /  leLspeed; 
if(  dist_to_cpa  <=  0 ) 

{ 

if  (!aS_AIRCRAFT(OBJ_OBJECT_TYPE(avoidcr)))) 

{ 

avoidcr->cntity->vclocity[0]  =  ol<Lvcl[0]; 
avoider->attity->velocity[l]  =  <dd_vel[l]; 

1 

return; 

} 

timejiojcpa = dist_to_cpa  /  rel_speed; 


/*  if  diere  is  a  goal  point,  calculate  time  to  it  */ 
if(  goal^t  !=  Null_Vcctor )  { 

^leed  s  vec2_inag(  avoider->entity->velociQr ); 
if(  speed  <  SMALL ) 
return; 

VEC2_SUB(  goal_pt,  avoider->cntity->position,  rel_pos_of_point ); 


dist;_tojpoiiit  *  vcc2_inag(  rel_pos_of_point ); 
tiine_!o_point  =  dist_l04)oint  /  ^)eed; 

/*  if  will  reach  goal  pdnt,  before  collision,  don't  do  avoidance  */ 
if(  time_to_point  <  tiine_to_cpa ) 
return; 

} 

I*  calculate  magnitude  in  change  of  velocity  in  direction  normal  to 
relative  velocity  away  from  collisitm  *! 
fsf = fu2zy_scale_factor(Passing_I>ist,  dist_to_cpa,  ^>eed); 

/*  if  avoider  is  not  an  aircraft  and  the  scale  factor  is  not  too  small  slow 
down  ♦/ 

if(!(IS_AIRCRAFT(OBJ_OBJECT_TYPE(avoidcr)))) 

{ 

if(fsf>l  .0)  vcc2_div(fsf,avoidcr->cntity->velocity,desired_vel); 

} 

scale.factor  = 

SGN(  disuaucpa )  ♦  AVOID_FACTOR  ♦  fsf; 
delta_vel[  0  ]  =  rel_vcl[  1  ]  *  scale.factcnAeLspeed; 
dclta_vel[  1  ]  =  •  rel_vel[  0  ]  *  scale_factorAel_speed; 


VEC2_ADD(  desiitdjvel,  dclta_vcl,  desired_vel ); 


1 


^**m************************************************************************/ 


5.  Fuzzy  Functions 


*  This  function  calculates  a  fiizzy  scale.factor  replacing  the  (me  used  in  the 
*SAFOR  function  avoid.c. 


4i*4i*4i****4i4i4i4i4i**4i4i«**iti4i******)|i4i******4>«*4i*4>********4»t<*it»t>****<li***4t«***4i4t*****4i/ 


float  fuzzy_scale_factQr(pd,  dcpa,  speed) 


#defineDCPAl  10.00 
#defineDCPA2  50.00 
«defineDCPA3  100.00 
^define  SCLFl  2.00 
«define  SCLF2  6.00 
«define  SCLF3  10.00 
«definePDNUM10 
«defineDCPANUM  10 
#define  SCFNUM  3 


float  pd,  dcpa,  speed; 
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{ 

float  mf_scale_factorO; 
intij.k; 

float  opd[4],  odq)a[4],  ipd[4][4],  rdcpa[4][4],  rsclfI4][4][4], 
scf_pt  =  0,  scf_inc,  scfjtnf,  term,  num.sum  =  0,  den_sum  =  0,  scalc_factor, 
inax_dq)a,  iiiax_jxl,  max.scf  =  SCXF3.  PDl,  PD2,  PD3; 
max.dqMt  =  8.0*speed; 

if  (Passing_Dist  =  DI_PASSING_DIST) 

{ 

PD1=0; 

PD2  =  5.00; 

PD3  =  10,00; 

) 

if  (Passing_Dist  =  GRND_PASSING_DIST) 

{ 

PDl  =  5.00; 

PD2  =  10.00; 

PD3  =  50.00; 

] 

if  (Passing_Dist  ~  HELO_PASSING_DIST) 

{ 
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PDl  »  10.00; 

PD2  *  50.00; 

PD3  =  100.00; 

) 

inaxjxi  =  PD3; 

I*  Enter  die  anay  eletncnte  defining  the  membership  functions  of  the  fuzzy  sets 

in  die  rules  that  determine  the  scale  factor.  *! 

rpd[l][l]  =  PD3; 

rpd[ll[2]=PDl; 

ipd[ll[3]  =PD2; 

rpd[2]tl]=PDl; 

rpd[2][2]*PD2; 

ipd[2][3]»PD3; 

ipd[3][l]=PD2; 

ipd[3][2]  =PD3; 

ipd[3][3]=  PDl; 

idci)a[l][l]=DCPA3; 

idcpall][2]  =DCPA1; 

nlcpa[l][31  =  DCPA2; 

idcpaI2][l]=DCPAl; 

idcpa[2][2]=DCPA2; 
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ntepa[2][31  =  DC3>A3; 
idcpaf3][l]*DC3>A2; 
idcpaI3][2]*DC3>A3; 
idcpaI3][3]=DC3*Al; 
rsclfIl][l][l]  =  SCLFl 
rsclf[l][l][2]  =  SCLF2 
rsclf[lKl][3]  =  S<XF3 
rsclfll][2][l]  =  SCLF3 
rsclf[l][2][2]  =  SCLFl 
rsclfll][2][3]  =  SCLF2 
rsclf[l][3][l]  =  SCLP3 
rsclf[l][3][2]  =  SCLFl 
rsclf[l][3][3]  =  SCLF2 
rsclfI2][l][l]  =  SCLF2 
rsclfl2][l][2]  =  SCLF3 
rsclfI2][l][3]  =  SCLFl 
rsclf[2][2][l]  =  SCLF2 
rsclfI2][2][2]  =  SCLF3 
rsclf[2][2][3]  =  SCLFl 
rsclfI2][3][l]  =  SCLFl 
rscIfT2][31[21  =  SCLF2 
rsclf[2][3][3]  =  SCLF3 
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» 


reclfI3][l][l]«SCLF2 
rsclf[3][l][2] «  SCLF3 
Tsclft3][ll[31  *  SCLFl 
rsclfl3][2][l]  =  SCLF2 
rsclfl3][2][2]  =  SCLF3 
rsclfI31[2][3]  -  SCLFl 
rsclfl3][3][l]  =  SCLFl 
rsclft3][3][2]  =  SCLF2 
rsclf[3][3][3]  =  SCLF3 


I*  Get  die  array  elements  defining  the  membership  functions  of  the  observed 
(i.e.,  ixescribed  for  the  object  to  be  avdded)  pasang  distance.  */ 
if(pd  =  PDl) 

{ 

•  opd[l]=PD3; 
opd[2I  =  PDl; 
opd[3]=PD2; 

} 

else 

•  { 

opd[l]=PDl; 

opd[2]=PD2; 
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opd[31=PD3; 

) 

/*  Get  the  airay  elements  defining  the  membership  functions  of  the  observed 
distance_to_cpa.  ♦/ 
if  (dcpa  <=  DCPAl) 

( 

odcpafl]  =  DCyA3; 
odcpa[2]  =  DCPAl; 
odcpa[3]  =  DCPA2; 
goto  FINISH; 

) 

if  (dcpa  >*  DCPA3) 

{ 

odcpa[l]  =  DCPA2; 
odcpa[2]  =  DCPA3; 
odcpa[3]  =  DCPAl; 
goto  FINISH; 

} 

odcpa[l]  =  DCPAl; 
odcpa(2]  =  DCPA2; 
odcpa(3]  =  DCPA3; 
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/*  Then  calculate  the  centroid  of  the  scale_factor  membership  function.  The 
centroid  is  the  "best  estimate"  of  the  actual  scale.factor  to  be  used. 

First  get  the  increment  scf_inc  of  the  scale_factor  membership  function's 
argument  scf  j)L  The  increment  will  determine  the  points  at  which  the 
membership  values  are  to  be  calculated.  */ 

FINISH:  scf_inc  =  max_sc£flSCFNUM; 
while  (scf j)t  <  max.scf) 

{ 

scfjJt  +=  scf_inc; 


/*  Use  the  function  scf_mf  to  calculate  the  scale_factor  noembership  function 
value  at  the  point  scf_pL  */ 

scf_mf  =  mf_scale_factor(opd,  odcpa,  ipd,  rdcpa,  rsclf,  max_pd, 
max_dcpa.  scf_pt); 


/*  Use  the  result  to  calculate  the  centroid  of  the  scale.faacr  membership 
function.  */ 

tenn  =  scf_mfscf_pt; 
num_sum  +» tenn; 
den_sum  +=  scf_mf; 

) 

if  (den_sum  >  0) 
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return  (num.suin/den.sum): 


else  ^ 

return  (0); 

} 


f*#*****m******************************************************************** 


*  This  function  calculates  the  membership  function  for  the  scale_factor,  given 

*  the  membership  functions  of  the  observed  passing_distance  obs_pd,  the  observed 

*  distance_to_cpa  obs.dcpa,  and  3  modens  ponens  rules  that  prescribe  the 
scale.facuv  for  each  ccxnbination  of  the  passing_distance  pd  and  the 

*  distance_to_cpa  dcpa. 


float  mf_scale_factoi<obs_pd,  obs_dcpa,  pd,  dcpa,  sclfac,  max_pd,  max_dcpa, 
scf_pt) 


i 

float  obs_pd(41,  obs_dcpa[4],  pd[4][4],  dcpa[4][4],  sclfac[4][4][4],  max_pd, 
max.dcpa,  scf_pt; 

( 

{ 

float  con^sitionO; 

int  ijjt;  ^ 

float  qxi[4],  odcpa[4],  rpd[4],  rdcpa[4J,  rsclfl4],  comp_val,  max_val  = 


/*  Get  all  obs_pd  and  obs_dq)a  array  components.  ♦/ 
for  (i  =  1;  i  <  4;  i++) 

{ 

opd[i]  =  obs_pd[i]; 
odcpa[i]  =  obs_dcpa[i]; 

} 

/*  Get  the  membership  function  value  of  the  scale.factor,  which  is  the  union  of 
the  sets  determined  by  each  of  the  rules  associated  with  all  possible 
combinations  of  a  pd  and  a  dcpa.  Start  by  getting  the  required  array 
components.  *! 
for  (i  =  1;  i  <  4;  i++) 

{ 

^0  =  l;j<4a++) 

{ 

for  (k  =  1;  k  <  4;  k++) 

ipd[k]=pd[i][k]; 
rdcpafk]  =  dcpalj][k]; 
rsclf[k]  =  sclfac[i][jl[kl; 

) 
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I*  The  result  of  each  rule  is  detennined  by  the  composition  function.  *! 

comp.val  =  composition(opd,  odcpa,  rpd,  rdcpa,  rsclf, 
max_pd,  max.dcpa,  scf_pt); 


I*  Then  calculate  the  union  membership  function  value.  *! 
if  (oomp_val  >  max_val) 

max_val  =  comp_val; 


return  (max_val); 


*  This  functicm  calculates  the  membership  function  of  the  intersection  of  the 

*  observed  passing_distance  obs_pd  and  the  observed  distance_to_dcpa  obs.dcpa, 

*  both  which  have  generic  membership  functions.  It  does  the  same  for  the 

*  corresponding  quantities  whose  intersection  is  the  modens  in  a  specific  modens 
ponois  rule;  die  passing_distance  rule_pd  and  the  distance_to_cpa  rule.dcpa, 

*  both  of  which  also  have  generic  membership  functions.  It  then  calculates  the 

*  relation  defined  by  the  rule  and  then  the  composition  of  the  intersecticm  of 

*  the  observed  quantities  with  the  relation. 
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float  c(xiq)Osition(obs_pd,  obs_dq)a,  rule_pd,  rulc_dq)a,  sclfac,  max_pd, 
maxjdqM,  scf.pt) 

float  obs_pd[4],  obs_dcpa[4],  rulc_pd[4],  rule_dcpa[4],  sclfac[4],  max_pd, 
inax_dcpa,  scf.pt; 

{ 

float  intersectionO.  membfuncO.  fuzzy.andQ; 
inti; 

float  opd[4],  odcpa[4],  pd.inc,  dcpa.inc,  pd_pt  =  0,  dcpa.pt  =  0, 
rpd[4],  idcpa[4],  mf.obs.dist,  mf.rule.dist,  mf.relation,  scf[4],  inf_scf, 
nile.mf ,  niax_val  =  0; 

/*  Hrst  get  the  increment  of  the  2  independent  variables  of  the  intersectitxi 
membership  functions  by  dividing  the  continuous  range  of  each,  maxjxl  and 
max.dcpa,  by  the  number,  PDNUM  and  DCPANUM,  of  discrete  values  to  be 
considered  in  eitiier  case.  Then  get  the  components  of  each  of  the  arrays 
involved.  ♦/ 

pd_inc  =  max jxi/PDNUM; 
dcpa Jnc  =  max.dcpa/DCPANUM; 
for  (i  as  1;  i  <  4;  i++) 

{ 

(^[i]  =  ob5.pd[i]; 


odcpa[i]  s  obs_dq)a[i]; 
ipd[i]»rulc_ixi[i]; 
nlq)a[i] «  rule_dq>a[i]; 
scf[i]  s  sclfac[i]; 

) 

I*  Get  the  value  of  die  scale_factor  membership  function  mf.scf,  which  is 
generic.  */ 

mf_scf  =  membfunc(scfll],  scf[2],  scfI3],  scf_pt); 

/*  Now  do  the  promised  composition  inference  calculation,  which  involves  a 
max-min  product  ♦/ 
while  (pd_pt  <  max_pd) 

( 

pd_pt  +=  pdjnc; 

while  (dcpa_pt  <  max.dcpa) 

{ 

dcpa_pt  +=  dcpa_inc; 

/*  Get  the  membership  function  mf_obs_dist  of  the  intersection  of  obs_pd  and 
obs.dcpa.  ♦/ 

tnf_obs_dist  ®  fuzzy_and(opd,  odcpa,  pd_pt  dcpa_pt); 
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/*  Get  the  membership  function  mf_rule_dist  of  the  intersection  of  rule_pcl  and 
Tule.dcpa.  */ 


mfjrulejdist  =  fuzzy_and(rpd,  rdcpa,  pd_pt,  dcpa_pt); 

f*  Get  die  membership  function  mf.relation  of  the  rule  relation,  which  is 
defined  by  the  intersection  of  mf.rule.dist  and  mf_scf.  */ 

mf_relation  =  intersection(mf_rule_dist,  mf_scf); 

/*  Get  the  membership  function  rule.mf  of  the  intersection  of  obs_dist  and 
mf.ielation.  ♦/ 


rulc_mf  =  intersection(mf_obs_dist,  mf.relation); 

I*  Finally,  get  the  composition  value,  which  is  defined  as  the  maximum  value  of 
rule_mf  for  all  points  pd_pt  and  depart.  *! 
if  (rulejmf  >  max_val) 

max.val  =  rulc_mf; 

} 

dcpa_pt  =  0; 

} 


return  (max.val); 


float  membfuncl,  nKmbfunc2; 

{ 

if  (membfuncl  <  membfunc2) 
return  (membfuncl); 
else 

return  (membfunc2); 

) 


*  This  function  detennines  the  value  at  a  point  p  of  the  generic  membership 

*  functitm  determined  by  the  parameters  pi .  p2,  p3. 


float  membfunc(pl,  p2,  p3,  p) 


float  pi,  p2,  p3,  p; 
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( 


float  MF; 

if  (pl<p2)  goto  NOT.SMALL; 
if(p<=p2) 


MF=1; 

goto  FINISH; 


goto  DOWN; 
NOT_SMALL:  if  (p<=pl) 


MF*0; 


goto  FINISH; 


if(p<p2) 


MF=(p-pl)/(p2-pl); 
goto  FINISH; 


DOWN:  if(p<p3) 


L 


MF*(p3-p)/(p3-p2); 
g<«o  FINISH; 


if(p3<p2) 


MF=1; 

goto  FINISH; 


if  (p>=p3) 

MF=0; 

FINISH:  return  (MF); 


*  This  function  determines  the  value  of  the  membership  function  of  the 
intersection  of  two  fuzzy  sets  having  generic  membership  functions. 

**4>**4i4(4i4i4>4>*4i4i4i*4'4i4i4i4i4i*4i4i4^4i4>*4i4i4i4>4>4i**4>*4>**4t4i4>4i4i4i*4>4i4i4>4>*4>4r4c4i4>4<4>4[4i4<*4>4i4i4>4>4i4i4(4i4>4>^ 

float  fuzzy_and(membfl,  membf2,  ptl,  pt2) 
float  membfl[4],  membf2[4],  ptl,  pt2; 
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float  par[4],  memfl,  ineinf2,  MF; 


for  (i  *  1;  i  <  4;  i++) 


( 

par[i]  s  membflp]; 

} 

memfl  =  membfunc(par[l],  par[21,  par[3],  ptl); 
for  (i  *  1;  i  <  4;  i++) 

{ 

paifi]  s  membf2[i]; 

} 

mem£2  -  membfunc(par[l],  par[2],  par[3],  pt2); 
MF  s  intersection(memfl,  memf2); 
return  (MF); 

) 


y4ii|i*4i4i*4i*4i*4i4i  4i*4i4i4i4i  4i  4t4r4i4i4i  4i**4i4i4i*4i  4i4r4^4i4i4i4ii|i**«4ii|i4i4i4i*4i4i  g|i4i4i4i  41*41 4i4i4i4i4i4i4i4i4ii|i4i4i4i4i4i4i^ 
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GW  BASIC  OBSTACLE  AVOIDANCE  PROGRAMS 
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APPENDIX  D 

GW  BASIC  OBSTACLE  AVOIDANCE  PROGRAMS 


0  TOOGRAM  TO  AVOID  SPHERICAL  OBJECTS  USING  A  CRISP  ALGORITHM 

10  TNmALIZE  THE  COLLISION  ANALYSIS  STATUS  LAST.  DISH-AY  ONLY  WHEN  LAST=1. 

20LAST^ 

30  INPUT  "USE  PARAMETERS  IN  FILE  Y(N)";ANS$ 

40  IF  ANS$«"Y"  OR  ANS$«"y"  THEN  OTON  "I*.l."OBSTACLE.DAT"  ELSE  GOTO  80 
50  *YES.  SO  GET  THE  PARAMETERS  FROM  THE  FILE  CALLED  •OBSTACLE.DAT. 

60GOSUB  1000:GOTO  140 

70  !«).  SO  GET  THE  PARAMETERS  DIRECTLY  FROM  THE  KEYBOARD. 

80  OPEN  "0".l,"OBSTACLE  J)AT" 

90  GET  THE  VEHICLE  RADIUS  AND  ITS  INITIAL  AND  DESTTNATTON  (X.YZ)  LOCATIONS. 

100  GOSUB  1500 

1 10  GET  THE  NUMBER  OF  OBSTACLES.  THEIR  RADH,  AND  THEIR  (X.YZ)  LOCATIONS. 

120  GOSUB  2000 

130  GPEN  A  FILE  TO  RECORD  THE  VEHICLE'S  OBSTACLE  AVOIDANCE  PATH. 

140  OPEN  "O"  A"VCOORDST)Ar 
150  XV»XV0:YV«YV0-.ZV=ZV0 

160  TIND  THE  OBSTACLE  COLLISION  POINTS  ON  THE  VEHICLE  PATH  FROM  ITS  INITIAL 
170  •LOCATION  TO  ITS  DESTINATION  AND  DETERMINE  THE  EARLIEST  ONE. 

180  GOSUB  3500 

190  TF  NO  COLLISIONS  OCCUR  PRINT  THAT  MESSAGE  AND  QUIT. 

200  IF  MINI«0  THEN  PRINT  "NO  OOLUSIONS"a»RINTGOTO  690 

210  TF  MINI>0  A  COLLISION  WITH  OBSTACLE  NUMBER  MINI  WILL  OCCUR. 

220  irND  THE  VEHICLE  POSITION  AT  THE  COLUSION  POINT.  IF  THE  COLLISION  POINT 
230  TS  FURTHER  TO  THE  LEFT  THAN  THE  INITIAL  VEHICLE  POSITION  AN  OBSTACLE  IS 
240  TOO  CLOSE  TO  THE  VEHICLE  POSITION.  ANNOUNCE  THE  FACT  AND  QUIT.  OTHERWISE 
250 'CONTINUE. 

260  GOSUB  4000:IF  XA<XV0  THEN  GOTO  9040  ELSE  DXA*XA-XD:DYA=YA-YD 
270  TF  NO  MORE  OBSTACLES  ARE  AHEAD  MOVE  AND  DISPLAY  THE  VEHICLE. 

280  IF  LAST«1  THEN  GOSUB  6500 

290  'OTHERWISE  EXAMINE  THE  OBSTACLES  AHEAD.  IF  MORE  niAN  TWO  OBSTACLES  IN  THE 
300  'VEHICLES  PATH  ARE  TOO  CLOSE  TOGETHER  FOR  THE  VEHICLE  TO  PASS  BETWEEN  THEM 
310  THEN  THE  REMAINING  CONFIGURATION  OF  OBSTACLES  IS  DIFFERENT  FROM  THE 
320  'ORIGINAL  ONE.  SO  REINITIALIZE  THE  VEHICLE  POSITION  AND  START  OVER. 

330  GOSUB  4500:IF  NC>1  THEN  GOSUB  2500:GOTO  150 

340  TF  NO  TWO  OBSTACLES  IN  THE  VEHICLE'S  PATH  ARE  TOO  CLOSE  TOGETHER  FOR  THE 
350  'VEHICLE  TO  PASS  BETWEEN  THEM  THEN  CONTINUE  WITH  THE  SAME  OBSTACLE 
360  XGNHGURA'nON. 

370  IF  NC=0  THEN  460 

380  XJTHERWISE  CHOOSE  THE  BEST  SIDE  OF  THE  OBSTACLE  FOR  THE  VEHICLE  TO  GO 
390  'AROUND  AND  MOVE  THE  VEHICLE  TO  THE  OBSTACLE  AND  AROUND  IT  ON  THAT  SIDE. 


400  IF  (DY(CD>DYD1)  OR  (DY(CD>DYD2)  THEN  430 
4I0IFDYD2<DYD1  THEN  XV=XI>4-DXDl:YV=YD+DYDl:GOTO  520 
420  GOTO  500 

430  IF  (DY(CI)<DYD1)  OR  (DY(CI)<DYD2)  THEN  460 

440  IF  DYD2>DYDI  THEN  XV=XD<-DXDl:YV=YDfDYDl:GOTO  520 

450  GOTO  500 

460P21«SQR(DXDl''2+DYDl''2>fS(?l((DXA-DXDiy'2+(DYA-DYDl)*2) 

470  R22«S<?l(DXD2>'2+DYD2'^2>fS(^l((DXA-DXD2)^2-KDYA-DYD2y^2) 

480  OIVEN  A  CHOICE.  CHOOSE  THE  SHORTEST  DISTANCE  AROUND  THE  OBSTACLE. 

490  IF  R21<R22  THEN  XV»XDfDXDl;YV=YD+DYDl:GOTO  520 
500  XV»XDf  DXD2:  YV=YDf  DYD2 

510  ’MAKE  SURE  THE  VEHICLE  LOCATION  IS  TO  THE  RIGHT  OF  ITS  INITIAL  POSITION. 

520  IF  XV<XV0  THEN  XV=XV0:  YV=YV0 

530  ■MOVE  THE  VEHICLE  AROUND  THE  OBSTACLE  AND  DISHJ^Y  THE  MOTION  ON  SCREEN. 
540  GOSUB  7500 

550  TF  THERE  IS  A  COLLISION  AT  ALL  CHECK  FOR  NEXT  COLLISION. 

560  IF  MINI>0  THEN  GOSUB  3500 

570  TF  NO  REMAINING  COLLISION  CAN  OCCUR  DISPLAY  THE  VEHICLE  MOVE  TO  THE 
580  'DESTINATION  AND  QUIT. 

590  IF  (LAST=1  AND  MINI*0  AND  XA<XD)  THEN  XA=XD:YA=YD:GOSUB  6500:GOTO  690 
600  IF  NO  COLUSION  IS  HIEDICTED  THE  OBSTACLE  CONHGURATION  HAS  BEEN 
610  'MODIFIED  BY  BEING  COMBINED  INTO  A  SINGLE  OBSTACLE.  SO  START  OVER. 

620  IF  (LAST-0  AND  MINI-0)  THEN  LAST=I:GOTO  150 

630  TS  THE  COSDITlOti  FOR  COLUSION  WITH  OBSTACLE  NUMBER  MINI  SATISFIED? 

640  TF  SO  REPEAT  THE  AVOIDANCE  PROCESS  WITH  THE  PRESENT  OBSTACLE 
650  CONFIGURA’nON. 

660  IF  D(MINI)<=  RSUM2(MINI)  THEN  260 

670  TF  NOT  DISH-AY  THE  VEHICLE  MOVE  TO  THE  DESTINATION  AND  QUIT. 

680  XA=XD:YA=YD:GOSUB  6500 
690  IF  INKEYS-"-  THEN  690 
700  SCREEN  2;SCREEN  0:CLOSE  #I 
710  END 
720’ 

1000  'GET  THE  PARAMETERS  FROM  THE  FILE. 

1010  I=0:INPI]T  #I.XV0.YV02V041V 
1020  INPUT  #1.XD.YD.ZD 
1030  WHILE  NOT(EOF(l)) 

10401-I-t-l 

1050  INPUT  #l.X(D.Y(I)2(I)Jl(D:Xl(I)=X(I):Yl(I)=Y(I)21(I)=Z(I):Rl(I)=Ra) 

1060  WEND 

1070  'COUNT  THE  NUMBER  OF  OBSTACLES  NO. 

1060  NC^kNOB-NaNOBO^OB 

1090  "NOTE  THAT  THE  DATA  IS  INPUT  FROM  A  FILE. 


nooFE4»“r 

1  no  •GET  THE  INITIAL  DISMAY  OF  THE  VEHICLE  AND  OBSTACLES  ON  SCREEN. 

II20GOSUB  2100 
1130  RETURN 
1140' 

1500  INPUT  THE  VEHIClJE  PARAMETERS  FROM  THE  KEYBOARD. 

1510  INPUT  "VEHICLE  INITIAL  X.YZ  COORDINATES";XVO.YVO^VO 
1520  INPUT  “VEHICLE  RADIUS’^IV 

1530  INPUT  "DESTINATION  X.Y^  COORDINATES";XD.YDZD 

1540  TF  THE  DATA  COMES  FROM  A  FILE  THEN  DONT  PRINT  IT  TO  A  FILE. 

1550  IFFIL$«"r  THEN  1570 

1560  PRINT  #I^0,YV0^03VJ*RINT  #I^.YD;n> 

1570  RETURN 
1580' 

2000  'GET  THE  OBSTACLE  DATA  FROM  THE  KEYBOARD. 

2010  INPUT  "NUMBER  OF  OBSTACLES"^«):NOB=NO-JJOBO=NOB 

2020  WHILE  I<NO 

2030I>=I+I 

2040  PRINT  "OBSTACLE  #";I 

2050  CH-0;INPUT  "X.Y;5  C00RDINATES";X(I).Y(D2(I) 

2060  INPUT  "RADIUS“;R(I):IF  I>I  THEN  GOSUB  8500:1F  CH=I  THEN  2050 
2070  'SAVE  THE  PARAMETERS  IN  THE  DESIGNATED  RLE. 

2080  PRINT  #I^(D.Ya)Z(DJia):XI(D=X(D;YI(I>=Y(D2:i(D=Z(I):Ria>=R(I) 

2090  WEND 

2100  'PRINT  IHE  FIRST  OBSTACLE  DIAGRAM  ON  THE  SCREEN. 

21 10  GOSUB  6000 

2120  'ADD  THE  VEHICLE  IN  ITS  INITIAL  POSmON. 

2130  XX«XV0:YY=YV0-JW»RV:GOSUB  7000 
2140  IF  INKEY$»""  THEN  2140 
2150  RETURN 
2160' 

2500  'REASSIGN  OBSTACLE  NUMBERS  AND  PARAMETERS  AFTER  COMBINING  2  OBSTACLES. 
2510  'ZERO  THE  OBSTACLE  FLAG.  IF  FEWER  THAN  2  OBSTACLES  REMAIN  NO  MORE 
2520  'OBSTACLE  COMBINING  IS  NEEDED. 

2530  OBaO:IF  NO<2  THEN  2780 

2540  TF  MORE  THAN  1  OBSTACLE  IS  PRESENT  CHECK  IF  TWO  OBSTACLES  ARE  CLOSE 
2550  ENOUGH  TO  BE  COMBINED  INTO  ONE.  IF  SO  COMBINE  THEM.  SET  FLAGS,  UPDATE 
2560  "THE  OBJECT  COUNTERS. 

2570  FOR  I»2  TO  NO 
2580  FOR  J=1  TOM 

2590RHO2aJMX(I>X(J)y'2+(Y(I)-Y(J)y'2+(Z(I)-Z(J)y'2:RHOaJ)=SQR(RHO2aJ)):RU=R(l>fR(J) 

2600  DROa  J)*RHOa  J>(RU+2*RV) 

2610  IF  DRO(IJ)<0  THEN  GOSUB  3000:W(I)-1:OB«1  J*I:I*NO+l 
2620  NEXT 
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2630  NEXT 

2640  ’AFTER  COMBINING  TWO  OBSTACLES  REASIGN  OBSTACLE  NUMBERS.  OR  RETURN  IF  NONE 
26S0  'WERE  COMBINED. 

2660  IF  OB-1  THEN  JsO  ELSE  GOTO  2860 
2670  FOR  1-1  TO  NO 

2680  •SKIP  rrS  NUMBER  IF  AN  OBSTACLE  IS  REMOVED  BY  COMBINING  IT  WITH  ANOTHER 
2690 'OBSTACLE. 

2700  IF  W(I)-1  THEN  NOB»NOB-1:GOTO  2720 
2710J»I+1:X(J)-X(I):Y(J>=Y(I)2(J>^:R(J)=R(I) 

2720  NEXT 

2730  IF  THE  PRESENT  NUMBER  OF  RECOGNIZED  OBSTACLES  AGREES  WITH  THE  NUMBER 
2740  UaFT  AFTER  COMBINING  PAIRS  AND  MORE  THAN  1  IS  LEFT,  CHECK  OUT  THE  NEW 
2750  CONFIGURATION. 

2760  IF  NOB-NO  THEN  2850 

2770  TF  ONE  OR  MORE  OBSTACLES  WERE  REMOVED  RESET  THE  NUMBER  OF  OBSTACLES. 
2780NO«NOB 

2790  'GET  THE  DIAGRAM  ON  THE  SCREEN  IF  THE  LAST  OBSTACLE  HAS  BEEN  ENCOUNTERED. 
2800  IF  LAST-1  THEN  GOSUB  6000:ELSE  2830 

2810  IF  MORE  THAN  1  OBSTACLE  REMAINS  OR  NO  MORE  THAN  1  OBSTACLE  EXISTED  BEFORE 
2820  THE  LATEST  COMBINATION  OF  OBJECTS  TOOK  PLACE  THEN  R’^TURN. 

2830  IF  NO>l  THEN  2860 

2840  TF  MORE  THAN  1  OBSTACLE  IS  LEFT  CHECK  OUT  THE  NEW  CONHGURATTON. 

2850  IF  NOB>l  THEN  2530 

2860  RETURN 

2870’ 

3000  ’GET  THE  POSITION  COORDINATES  AND  RADIUS  OF  THE  NEW  OBSTACLE  FORMED  BY 
3010  'COMBINING  2  OLD  ONES. 

3Q20CI>-2*RHOaJ):FAa-(RHO(U>4-R(D-R(J)VCD:FAa-(RHOaJ)+R(J)-R(D)/CD 

3030X(J)-FACT*X(I)+FAa*X(J):Y(J)=FAa*Y(l)+FAa*Y(J)2(J)=FACI*Z(I)+FACJ*Z(J) 

3040  R(J)-(RHOa  J>fR(IHR(J)y2 

3050  RETURN 

3060’ 

3500  'GET  THE  OBSTACLE  COLLISION  POINTS  ON  THE  VEHICLE  PATH  TO  ITS  DESTINATION. 

3510  CALCULATE  THE  COMPONENTS  OF  THE  VECTOR  FROM  THE  VEHICLE’S  INITIAL 
3520  'LOCATTON  TO  ITS  DESTTNATTON. 

3530  DXD-XV.XD;DYD-YV.YD:DZD=ZV-ZD 

3540  'CALCULATE  THE  DIRECT  VEHICLE  PATH  LENGTH  SQUARED. 

3550  DEN-DXD*DXI>f  DYD*DYI>fDZD*DZD 
3560  MIND-DEN:MINI-0 
3570  FOR  I-l  TO  NO 

3580  'CHECK  IF  VEHICLE  IS  HEADED  TOWARD  THE  OBSTACLE.  IF  NOT  SKIP  THE  COLUSION 
3590  TEST. 

3600  IF  .01-KX(I>XV)*DXDKY(D-YV)*DYTH<Z(I)-ZV)*DZD>-0  THEN  3770 
3610  DX(I>.X(I)-XD:DY(I)=Y(I)-YD:DZ(I)=Z(I)-ZD 
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3620  DD(D»DX(I)*DX(DfDYa)*DY(I>fDZ(I)*DZ(I) 

3630NUM-DXD*DX(I)+DYD*DY(I>fDZD*DZa):RSUM=RV+R(I):RSUM2a>=RSUM*RSUM 
3640  FAONUM/DEN:D(I)>DD(I>FAC*NUM 

3650  DO)  IS  THE  SQUARE  OF  THE  DISTANCE  OF  THE  CENTER  OF  OBSTACLE  I  FROM 
3660  THE  VEHICLE  PATH. 

3670  X0(I)«XI>fFAC*DXD:YO(I)=YI>fFAC*DYD2O(I>=ZD+FAC*DZD 

3680  7C0(D.Y0(I)20(I)  ARE  COORDINATES  OF  THE  POINT  ON  THE  VEHICLE  PATH 

3690  CLOSEST  TO  THE  CENTER  OF  OBSTACLE  1. 

3700  DISTOHNUM-DENy'a/DEN 

3710  •DISTO)  IS  THE  SQUARED  DISTANCE  FROM  THE  INITIAL  VEHICLE  LOCATION 
3720  TO  THE  POINT  (X0(D.Y0(I)20(D). 

3730  IF  D(D>»RSUM2(I)  THEN  3770 

3740  IF  DIST(I)<MIND  THEN  MINI*I:MIND=DIST(I) 

3750  TF  MIND  IS  A  MINIMUM  THEN  THE  COLUSION  WITH  OBSTACLE  NUMBER  MINI  IS  THE 
3760  •EARLIEST. 

3770  NEXT 

3780  TF  MIND<DEN  THEN  THE  EARLIEST  COLLISION  POINT  IS  AT  •X0(MINI).Y0(MINI). 

3790  •ZOCMINI).  GET  DISTANCES  BETWEEN  OBJECT  MINI  AND  ALL  OTHER  OBJECTS. 

3800  FOR  I«1  TO  NO 
3810  IF  IsMINI  GOTO  3830 
3820 

RH02(MINIJO^X(I>X(MINI))^24<Y(I>Y(MINI))^2+(2(I>Z(MINB)^2JUIO(MINIJ)=SQR(RH02(MINIJ)) 
3830  NEXT 
3840  RETURN 
3850* 

4000  CETTHE  LAST  VEHICLE  POSITION  BEFORE  WHICH  COLLISION  CAN  BE  AVOIDED. 

4010  LB»RSUM2(MIND-D(MINI) 

4020  FAC2>SQR(LB/DEN) 

4030  XA-X0(MINI)+FAC2*DXD:YA^Y0(MINI)+FAC2*DYD2A=Z0(MINI)+FAC2*DZD 

4040  RETURN 

4050’ 

4500  'GET  THE  NEW  VEHICLE  POSITION  AFTER  GOING  AROUND  AN  OBSTACLE  TO  AVOID 
4510 ’A  COLUSION. 

4520  'CHECK  IF  A  COLUSION  WILL  OCCUR  WHILE  THE  VEHICLE  GOES  AROUND  AN 
4530  OBSTACLE.  IF  MORE  THAN  1  SUCH  COLUSION  IS  PREDICTED  RETURN. 

4540  GOSUB  5500:IF  NOl  THEN  4610 

4550  OALCULATE  THE  VEHICLE  POSITION  COORDINATE  INCREMENTS  IN  GOING  AROUND  THE 
4560 'OBSTACLE. 

4570  L2=DD(MINI)-RSUM2(MIND 

4580  IF  ABS(DX(MINI))>0  AND  ABS(DY(MINI)>0  THEN  GOSUB  5000:GOTO  4610 

4590  IF  ABS(DX(MINI)>ABS(DY(MINI))  THEN  GOSUB  5300:0010  4610 

4600DYD1»L2^Y(MINI):DYD2=DYD1:DXD1=SGN(DXD)*SQR(L2-DYD1*DYD1)J)XD2»-DXD1 

4610  RETURN 

4620' 


SOOO  DEI^M«DIXMI^^)-OZ(MI^^)*DZ(MINI) 

5010  TERM2-SGN(DXD)*DY(M1NI)*SQR(L2*DEN0M-L2*L2) 

5020  TCimi-L2*DX(MINI):DXDl<rERMl+TtRM2)/DENOM:DXD2=(TERMl  -TCRM2)/DEN0M 
5030DYDlKL2-DX(MINI)*DXDiyDY(MINI);DYD2*(L2-DX(MlNI)*DXD2)/Dy(MINI) 

5040  RETURN 
5050’ 

5300DXDl^-2/DX(MINI):DXD2=DXDl:DYDl=SGN(DYD)*SQR(U-DXDl*DXDl):DYD2=»-DYDl 

5310  RETURN 

5320* 

5500  TIND  THE  OBSTACLE  PAIRS  IN  THE  VEHICLE  PATH  THAT  ARE  TOO  CLOSE  FOR  THE 
5510  •VEHICLE  TO  PASS  BETWEEN  THEM. 

5520  RMV»R(MINI>f  2*RVJ4C»0 
5530  FOR  WTO  NO 
5540  IF  I-MINI  THEN  5560 

5550  IF  RHO(MINI.I)<RMV+R(I)  THEN  NC=NC+1:CI=I 
5560  NEXT 
5570  RETURN 
5580’ 

6000  'GET  THE  BASIC  SCREEN  DIAGRAM. 

6010  CLS:SCREEN  1 

6020  LINE(XVO^YVO)-(XD;200-YD):CIRCLE(XD^YD)JIV,1 

6030PORC=1TONOBO 

6040  CIRCLE(X1(QWY1(Q)JI1(C).„.1 

6050  NEXT 

6060  RETURN 

6070' 

6500  •DISPLAY  THE  VEHICLE  MOVING  TO  THE  COLLISION  POINT  AT  AN  OBSTACLE  AND 
6510  •RECORD  THE  VEHICLE  PATH  POSITION  AND  RADIUS  AND  OBSTACLE  RADIUS  IN  THE 
6520  OUTPUT  FILE. 

6530  DXV=(XA-XVyi0:DYV=(YA-YVyi0 

6540  FOR  WTO  10 

6550  XX=XV:yY=YVJW=RV 

6560  IF  LAST®0  THEN  6580  ELSE  GOSUB  6000:GOSUB  7000:PRINT  #2XV,YV  JlV4l(MINI) 

6570  XV=XV+DXV:YV=YV+DYV:IF  XV<XV0  THEN  XV=XV0:  YV=YV0 
6580  •CHECK  FOR  A  COLLISION 
6590  GOSUB  8050 

6600  GOSUB  6000:GOSUB  7000:PRINT  #2,XV,YVJRV  Jl(MINI) 

6610  NEXT 

6620  XX=XV:  YY=YV  JRINT  #2^.YV  JlV4l(MINI) 

6630  GOSUB  6000:GOSUB  7000:PRINT  #2^V.YVJIV41(MIN1) 

6640  RETURN 
6650’ 

7000  •DRAW  THE  VEHICLE. 

7010  CIRCLE(XX^YY)JRR^„.l 
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7020  RETURN 
7030* 

I  7500 'MOVE THE  VEHICLE  AROUND  AN  OBSTACLE. 

7510DXX-XX-X(MINI):DYY*YY-Y(MINI):DXV«XV-X(MINI):DYV=YV-Y(MINI) 

7520  ROBi«SQR((X(MINI)-XAy'2+(Y(MINI)- YAy'2) 

7530  CALCULATE  THE  VEHICLES  ANGULAR  POSITION  RELATIVE  TO  THE  OBSTACLE. 

7540  AB>DXX.<»I>sOYY:GOSUB  8000:TH1»THO 
.  7550  AB->DXVK)RD-DYVKjOSUB  8000:TH2»TH0 

^  75eODTH»(TH2-THiy5:TH=THl 

7570  FOR  0-1  TO  5 
7580TH-TH+DTH 

7590  XX«X(MINI)4ROB*OOSaH):  YY»Y(MINI>f ROB*SIN(TH) 

7600  THEN  REFRESH  SCREEN  DISPLAY. 

•  7610  IF  LAST-0  THEN  7620  ELSE  GOSUB  6000:GOSUB  700(hPRINT  #2^.YY  JIV  Jl(MIND 
7620  NEXT 

7630  IF  LAST-0  THEN  7650 
7640  TF INKEYS-*"  THEN  7130 
7650  RETURN 

•  7660* 

8000  'CALCULATE  THE  ANGLE. 

8010  TH(WA1N(ORD/AB) 

8020  IF  SGN(AB>=-1  THEN  THft*THOf 3.1415926#:GOTO  8040 
8030  IF  SGN(ORD)-l  THEN  THO»THOf6.2831852# 

•  8040RETURN 

8050  XnECK  FOR  A  COLLISION  WITH  ANY  OBSTACLE. 

8060  PCXl  OBNUM-1  TO  NO 

8070DXY»(XV-Xl(C«NUM)y'2KYV-Yl(OBNUM)y'2+(ZV-ZI(OBNUM))^'I»R=(RV4Rl(OBNUM))^2 
8080  IF  .01+ DXY<I»R  THEN  9000 

•  8090NEXT 
8100  RETURN 
8110’ 

8500  IF  S<»((X(I>Xa'l))^2+(Y(I)-Ya-l))^2+<Z(I)-Za4))A2)<R(I)+Ra*l)  THEN  CH-l:PRINr  "OBJECT  TOO 
CLOSE  TO  THE  LAST  ONE." 

^  8510  RETURN 

^  8520' 

9000  SCREEN  2:SCREEN  0:PRINT  "COLUSION!!!":raiNT 

XV;YVZV^1(0BNUM);Y1(0BNUM)21(0BNUM)J)XYJ)RR 
9010  PRINT  #2..1.-14>XYJ)RR:PRINT  #24.  X1(D,Y1(I);PRINT  #2."COLUSION!!!" 

9020  IF  INKEYS-""  THEN  9020 

•  9030  XVO»XA:YVO-YA:GOTO  9080 

9040  SCREEN  2:SCREEN  0:PRINT  "OBSTACLES  TOO  CLOSE  TO  VEHICLE  STARTING  POINT. 

9050  PRINT  "COLLISION  OCCURS  AT  X  =";XA;",  Y  -";YA;".  Z  -"ZA;"." 

9060  IF  INKEYS-""  THEN  9060 
9070  GOSUB  6000:GOTO  9110 
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9080PORC«1TONOBO 
9090  CIRCLE(X(O^Y(C))  Jl(C) 

9100  NEXT 

9110  XX»XVO:YY-YVO-JIR-RVKJOSUB  7000 
9120  IF  INKEYS-"  THEN  9120 
9130  SCREEN  2:SCREEN 


0  TRCX3RAM  TO  AVOID  CYLINDRICAL  OBJECTS  USING  A  CRISP  ALGORITHM 

10  TNITTAUZE  THE  COLUSION  ANALYSIS  STATUS  LAST.  DISPLAY  ONLY  WHEN  LAST=1. 

20LAST=0 

30  INPUT  "USE  PARAMETERS  IN  FILE  Y(N)";ANS$ 

40  IF  ANS$»"Y"  OR  ANS$="y"  THEN  OPEN  "r.l,"OBSTACLE.DAT"  ELSE  GOTO  80 
50  TES,  SO  GET  THE  PARAMETERS  FROM  THE  FILE  CALLED  OBSTACLE.DAT. 

60GOSUB  1000:GOTO  140 

70  'NO.  SO  GET  THE  PARAMETERS  DIRECTLY  FROM  THE  KEYBOARD. 

80  (MEN  "0",l."OBSTACLE.DAT" 

90  'GET  THE  VEHICLE  RADIUS  AND  ITS  INITIAL  AND  DESTTNATTON  (X.Y2)  LOCATIONS. 

100  GOSUB  1500 

110  “GET  THE  NUMBER  OF  OBSTACLES.  THEIR  RADH,  AND  THEIR  (X.Y^)  LCX:ATT0NS. 

120  GOSUB  2000 

130  'OPEN  A  FILE  TO  RECORD  THE  VEHICLE'S  OBSTACLE  AVOIDANCE  PATH. 

140  OPEN  "O"  A-VCOORDSDAT 
150  XV*XV0:YV«YV(hZV»ZV0 

160  'FIND  THE  OBSTACLE  COLLISION  POINTS  ON  THE  VEHICLE  PATH  FROM  ITS  INITIAL 
170  T-OCATTON  TO  ITS  DESTTNATTON  AND  DETERMINE  THE  EARLIEST  ONE. 

180  CX)SUB  3500 

190  IF  NO  COLLISIONS  OCCm  PRINT  THAT  MESSAGE  AND  QUIT. 

200  IF  MINIaO  THEN  PRINT  "NO  OOLUSIONS"J»RINTGOTO  690 

210  IF  MINI>0  A  COLLISKMI  WITH  OBSTACLE  NUMBER  MINI  WILL  (XXTMl. 

220  TIND  THE  VEHICLE  POSITION  AT  THE  COLUSION  POINT.  IF  THE  COLLISION  POINT 
230  TS  FURTHER  TO  THE  LEFT  THAN  THE  INITIAL  VEHICLE  POSITION  AN  OBSTACLE  IS 
240  TOO  CLOSE  TO  THE  VEHICLE  POSITION.  ANNOUNCE  THE  FACT  AND  QUIT.  OTHERWISE 
250 'CONTINUE. 

260  (30SUB  4000:IF  XA<XV0  THEN  GOTO  9040  ELSE  DXA=XA-XD:DY A=YA-YD 
270  IF  NO  MORE  OBSTACLES  ARE  AHEAD  MOVE  AND  DISPLAY  THE  VEHICLE. 

280  IF  LASTsl  THEN  GOSUB  6500 

290  'OTHERWISE  EXAMINE  THE  OBSTACLES  AHEAD.  IF  MORE  THAN  TWO  OBSTACLES  IN  THE 
300  'VEHICLE'S  PATH  ARE  TOO  CLOSE  TOGETHER  FOR  THE  VEHICLE  TO  PASS  BETWEEN  THEM 
310  THEN  THE  REMAINING  CONFIGURATION  OF  OBSTACLES  IS  DIFFERENT  FROM  THE 
320  'ORIGINAL  ONE,  SO  REINITIALIZE  THE  VEHICLE  POSITION  AND  START  OVER. 

330  GOSUB  4500:IF  NOl  THEN  GOSUB  2500:GOTO  150 

340  IF  NO  TWO  OBSTACLES  IN  THE  VEHICLE'S  PATH  ARE  TOO  (LOSE  TOGETHER  FOR  THE 
350  'VEHICLE  TO  PASS  BETWEEN  THEM  THEN  CONTINUE  WITH  THE  SAME  OBSTACLE 


360m4FIGURATION. 

370  IF  NOO  THEN  460 

380  WHERWISE  CHOOSE  THE  BEST  SIDE  OF  THE  OBSTACLE  FOR  THE  VEHICLE  TO  GO 

390  'AROUND  AND  MOVE  THE  VEHICLE  TO  THE  OBSTACLE  AND  AROUND  IT  ON  THAT  SIDE. 

400  IF  (DY(CI>DYD1)  OR  (DY(CI)>DyD2)  THEN  430 

410  IFDYD2<DYD1  THEN  XV=XDfDXDl:YV=YDfDYDl:GOTO  520 

420  GOTO  500 

430  IF  (DY(CD<DYD1)  OR  (DY(CI)<DYD2)  THEN  460 

440  IF  DYD2>DYD1  THEN  XV.XDf  DXD1:YV=YD+DYD1:GOTO  520 

450  GOTO  500 

460R21*SQR(DXDl>'2fDYDl''2)fSQR((DXA-DXDiy'24<DYA-DYDiy'2) 

470 R22p«SQR(DXD2''2+DYD2'^2>fSQR((DXA-DXD2y'24(DYA.DYD2y'2) 

480  GIVEN  A  CHOICE.  CHOOSE  THE  SHORTEST  DISTANCE  AROUND  THE  OBSTACLE. 

490  IF  R21<R22  THEN  XV»XD4-DXDl;YV*YDfDYDl:GOTO  520 
500  XV.XDfDXD2:  YV*:YDf DYD2 

510  'MAKE  SURE  THE  VEHICLE  LOCATION  IS  TO  THE  RIGHT  OF  ITS  INITIAL  POSITION. 

520  IF  XV<XV0  THEN  XV«XV0:  YV=YV0 

530  •MOVE  THE  VEHICLE  AROUND  THE  OBSTACLE  AND  DISPLAY  THE  MOTION  ON  SCREEN. 
540  GOSUB  7500 

550  TF  THERE  IS  A  COLLISION  AT  ALL  CHECK  FOR  NEXT  COLLISION. 

560  IF  MINI>0  THEN  GOSUB  3500 

570  TF  NO  REMAINING  COLLISION  CAN  OCCUR  DISPLAY  THE  VEHICLE  MOVE  TO  THE 
580  ■DESTINATION  AND  QUIT. 

590  IF  (LAST=1  AND  MINI*0  AND  XA<XD)  THEN  XA»XD:YA*YD:GOSUB  6500:GarO  690 
600  TF  NO  COLUSION  IS  PREDICTED  THE  OBSTACLE  OONHGURATION  HAS  BEEN 
610  •MODIFIED  BY  BEING  COMBINED  INTO  A  SINGLE  OBSTACLE,  SO  START  OVER. 

620  IF  (LAST«0  AND  MINI«0)  THEN  LAST-IGOTO 150 

630  IS  THE  C(X'4Dm(X<<  FOR  COLUSION  WITH  OBSTACLE  NUMBER  MINI  SATISFIED? 

640  TF  SO  REPEAT  THE  AVOIDANCE  PROCESS  WITH  THE  PRESENT  OBSTACLE 
650  GONHGURA-nON. 

660  IF  D(MINI)o>  RSUM2(MINI)  THEN  260 

670  TF  NOT  DISnj^Y  THE  VEHICLE  MOVE  TO  THE  DESTINATION  AND  QUIT. 

680  XAsXD:  YAsYD:GOSUB  6500 
690  IF  INKEYS*""  THEN  690 
700  SCREEN  2:SCREEN  OcCLOSE  #1 
710  END 
720' 

1000  'GET  THE  PARAMETERS  FROM  THE  FOX. 

1010  I*0;INPUT  #1.XV0.YV0ZV0JIV 
1020  INPUT  #1.XD,YD.ZD 
1030  WHILE  NOT(EOF(l)) 

10401-1+1 

1050  INPUT  #1.X(I),Y(I)Z(I)3(I):X1(D=X(I);Y1(I)*Y(D*1(I)*R(I) 

1060  WEND 


1070  XX)UNT  THE  NUMBER  OF  OBSTACLES  NO. 

1000  NO>LNOBBNaNOBO»NOB 

1090  'NOTE  THAT  THE  DATA  IS  INPUT  FROM  A  FILE. 

1100FIL$»"r 

1 1 10  'GET  THE  INITIAL  DISPLAY  OF  THE  VEHICLE  AND  OBSTACLES  ON  SCREEN. 
I120GOSUB2100 
1 130  RETURN 
1140’ 

1500  INPUT  THE  VEHICLE  PARAMETERS  FROM  THE  KEYBOARD. 

1510  INPUT  "VEHICLE  INITIAL  X.YZ  COORDINATES";XVO,YVO;2VO 
1520  INPUT  "VEHICLE  RADIUS"aiV 

1530  INPUT  "DESTINA’nON  X.YZ  C00RDINATES";XD.YD2D 

1540  TF  THE  DATA  COMES  FROM  A  FILE  THEN  DONT  PRINT  rr  TO  A  FILE. 

1550  IF  FIL$»"r  THEN  1570 

1560  PRINT  #1^0.  YV0^V0JlV:PRINT  #1.XD.YD;ZD 

1570  RETURN 

1580' 

2000  ’GET  THE  OBSTACLE  DATA  FROM  THE  KEYBOARD. 

2010  INPUT  "NUMBER  OF  OBSTACLES";NO:NOB=NO:NOBO=NOB 

2020  WHILE  I<NO 

2030I«I-t-l 

2040  HUNT  "OBSTACLE  #";I 

2050  CH»0;INPUT  "X.YZ  C00RDINATES";X(I).Y(D7(I) 

2060  INPUT  *RADIUS";R(D:IF  I>1  THEN  GOSUB  8500:IF  CH*1  THEN  2050 
2070  •SAVE  THE  PARAMETERS  IN  THE  DESIGNATED  FILE. 
2080PRINT#1^(I).Y(I)2(I)JI(I):X1(I>X(I):Y1(I)=Y(I)21(I)=Z(I):R1(I)=R(I) 

2090  WEND 

2100  *PRINT  THE  FIRST  OBSTACLE  DIAGRAM  ON  THE  SCREEN. 

21 10  GOSUB  6000 

2120  'ADD  THE  VEHICLE  IN  ITS  INITIAL  POSITION. 

2130  XX=XV0;YY*YV0:RR=RV:GOSUB  7000 
2140  IF  INKEYS*"*  THEN  2140 
2150  RETURN 
2160' 

2500  'REASSIGN  OBSTACLE  NUMBERS  AND  PARAMETERS  AFTER  COMBINING  2  OBSTACLES. 
2510  'ZERO  THE  OBSTACLE  FLAG.  IF  FEWER  THAN  2  OBSTACLES  REMAIN  NO  MORE 
2520  'OBSTACLE  COMBINING  IS  NEEDED. 

2530  OB=0:IF  NO<2  THEN  2780 

2540  IF  MORE  THAN  1  OBSTACLE  IS  PRESENT  CHECK  IF  TWO  OBSTACLES  ARE  CLOSE 
2550  ENOUGH  TO  BE  COMBINED  INTO  ONE.  IF  SO  COMBINE  THEM,  SET  FLAGS,  UPDATE 
2560  "THE  OBJECT  COUNTERS. 

2570  FOR  1*2  TO  NO 
2580  FOR  J*1  TOM 

2590RHO2aJ)-(X(I)-X(J))A2+<Y(I>Y(J)y'2;RHOaJ)=SQR(RHO2(IJ)):RU=R(D+R(J) 


D-12 


2600  DROa  J)-RHO(IJHRU-t-2*RV) 

2610  IF  DRO(U)<0  THEN  CX)SUB  3000:W(D«.l;OB*l:J*I:I=NOf  1 
2620  NEXT 
2630  NEXT 

2640  'AFTER  COMBINING  TWO  OBSTACLES  REASIGN  OBSTACLE  NUMBERS.  OR  RETURN  IF  NONE 
26S0  'WERE  COMBINED. 

2660  IF  OB-I  THEN  J«0  ELSE  GOTO  2860 
2670  FOR  WTO  NO 

2680  'SKIP  ITS  NUMBER  IF  AN  OBSTACLE  IS  REMOVED  BY  COMBINING  IT  WITH  ANOTHER 
2690  OBSTACLE. 

2700  IF  W(D-1  THEN  NOB»NOB-1:GOTO  2720 
2710  W+1:X(J)«X(I):Y(J)»Y(I)*(D«R(I) 

2720  NEXT 

2730  IF  THE  PRESENT  NUMBER  OF  RECOGNIZED  OBSTACLES  AGREES  WITH  THE  NUMBER 
2740  'LEFT  AFTER  COMBINING  PAIRS  AND  MORE  THAN  1  IS  LEFT.  CHECK  OUT  THE  NEW 
2750  'CC»inGURATION. 

2760  IF  NOB»NO  THEN  2850 

2770  IF  ONE  OR  MORE  OBSTACLES  WERE  REMOVED  RESET  THE  NUMBER  OF  OBSTACLES. 

'  2780NO»NOB 

2790  'GET  THE  DIAGRAM  ON  THE  SCREEN  IF  THE  LAST  OBSTACLE  HAS  BEEN  ENCOUNTERED. 
2800  IF  LAST«I  THEN  GOSUB  6000:ELSE  2830 

2810  IF  MORE  THAN  1  OBSTACLE  REMAINS  OR  NO  MORE  THAN  1  OBSTACLE  EXISTED  BEFORE 
2820  THE  LATEST  COMBINATION  OF  OBJECTS  TOOK  PLACE  THEN  RETURN. 

I  2830  IF  NO>l  THEN  2860 

2840  IF  MORE  THAN  1  OBSTACLE  IS  LEFT  CHECK  OUT  THE  NEW  CONFIGURATION. 

2850  IF  NOB>I  THEN  2530 

2860  RETURN 

2870' 

I  3000  'GET THE  POSITION  COORDINATES  AND  RADIUS  OFTHE  NEW  OBSTACLE  FORMED  BY 

3010  'COMBINING  2  OLD  ONES. 

3020CD*:2*RHO(U):FAa*(RHOaj>R(I)-R(J)VCD:FAa»(RHOaj>+R(J>R(I))/CD 

3030X(J)=FAa*X(DfFAa*X(J);Y(J)«FAa*Y(I)+FAa*Ya) 

3040  R(J><RHOa  JHR(IHR(J)V2 
»  3050RETURN 

3060' 

3500  OET  THE  OBSTACLE  COLLISION  POINTS  ON  THE  VEHICLE  PATH  TO  ITS  DESTINATION. 

3510  CALCULATE  THE  COMPONENTS  OF  THE  VECTOR  FROM  THE  VEHICLE'S  INITIAL 
3520  'LOCATION  TO  ITS  DESTINATION. 

^  3530DXD=XV-XD:DYD=YV-YD 

3540  'CALCULATE  THE  DIRECT  VEHIOE  PATH  LENGTH  SQUARED. 

3550  DEN=DXD*DXD+DYD*DYD 
3560  MIND«CeN:MINI=0 
3570  FOR  I=I  TO  NO 

^  3580  'CHECK  IF  VEHICLE  IS  HEADED  TOWARD  THE  OBSTACLE.  IF  NOT  SKIP  THE  COLLISION 
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3590  "TEST. 

3600  IF  .01+(X(I)-XV)*DXD+<Y(D-YV)*DYT»-0  THEN  3770 
3610  DX(I>«X(I)-XD:DY(I^Y(I>YD 
3620DD(Di«DX(I)*DX(I)+DY(I)*DY(l) 

3630NUM-DXD*DXaHDYI>*DY(D:RSUM=RV+R(D;RSUM2(I)»RSUM*RSUM 
3640  FAC-NUM/I£N:D(I)-DD(I>FAC*NUM 

3650  *0(1)  IS  THE  SQUARE  OF  THE  DISTANCE  OF  THE  CENTER  OF  OBSTACLE  I  FROM 
3660  THE  VEHICLE  PATH. 

3670  X0(I)-XI>fFAC»DXD:Y0(D*YI>4-FAC*DYD 

3680  X0(I).Y0(I)  ARE  COORDINATES  OF  THE  POINT  ON  THE  VEHICLE  PATH 
3690  'CLOSEST  TO  THE  CENTER  OF  OBSTACLE  1. 

3700  DIST(IHNUM-DEN)A2/DEN 

3710  TOSTCD  IS  THE  SQUARED  DISTANCE  FROM  THE  INITIAL  VEHICLE  LOCATION 
3720  TO  THE  POINT  (X0a).Y0(I)). 

3730  IF  D(I)>»RSUM2(I)  THEN  3770 

3740  IF  DIST(D<MIND  THEN  MINI=I:MIND*DIST(0 

3750  TF  MIND  IS  A  MINIMUM  THEN  THE  COLUSION  WITH  OBSTACLE  NUMBER  MINI  IS  THE 
3760  EARLIEST. 

3770  NEXT 

3780  TF  MINIXOTN  THEN  THE  EARLIEST  COLLISION  POINT  IS  AT  X0(MIND.Y0(MINI). 

3790  OET  DISTANCES  BETWEEN  OBJECT  MINI  AND  ALL  OTHER  OBJECTS. 

3800  FOR  1*1  TO  NO 
3810  IF  I>MINI  GOTO  3830 

3820  RH02(MINUHX(I).X(MINI))^2+(Y(I>Y(MINn)^2:RH0(MM  J)*SQR(RH02(MINI  J)) 

3830  NEXT 
3840  RETURN 
3850* 

4000  GET  THE  LAST  VEHICLE  POSITION  BEFORE  WHICH  COLUSION  CAN  BE  AVOIDED. 

4010  LB*RSUM2(MINI>D(MIND 
4020  FAC2*SQR(LB/DEN) 

4030  XA«X0(MINIHFAC2*DXD:YA=Y0(MINI)+FAC2*DYD 

4040  RETURN 

4050' 

4500  'GET  THE  NEW  VEHICLE  POSITION  AFTER  GOING  AROUND  AN  OBSTACLE  TO  AVOID 
45I0'ACOLUSIC»I. 

4520  'CHECK  IF  A  COLUSION  WILL  OCCUR  WHILE  THE  VEHICLE  GOES  AROUND  AN 
4530  OBSTACLE.  IF  MORE  THAN  1  SUCH  COLLISION  IS  HIEDICTED  RETURN. 

4540  GOSUB  5500:IF  NC>1  THEN  4610 

4550  GALCULATE  THE  VEHICLE  POSITION  COORDINATE  INCREMENTS  IN  GOING  AROUND  THE 
4560 'OBSTACLE. 

4570  L2*DD(MINI)-RSUM2(MINI) 

4580  IF  ABS(DX(MINI))>0  AND  ABS(DY(MIND)>0  THEN  GOSUB  5000:GOTO  4610 

4590  IF  ABS(DX(MINI)>ABS(DY(MINI))  THEN  GOSUB  5300:GOTO  4610 

4600DYD1»L2/DY(MINI):DYD2*DYD1:DXD1=SGN(DXD)*SQR(L2-DYD1*DYD1):DXD2*-DXD1 


4610  RETURN 
4620' 

5000  DENOM-DD(MINI) 

5010  TERM2-SGN(DXD)*DY(MINr/  QR(L2*DENOM-L2*L2) 

5020 1ERM1«L2*DX(M1ND:DXT'  ;<rERMl+'reRM2)/DENOM:DXD2»(TERMl-TERM2yDENOM 
5030  DYDlKL2-DX(MINI)*DXDiyDY(MINI):DYD2*(L2-DX(MINI)*DXD2)/DY(MINl) 

5040  RETURN 
5050* 

5300DXD1=m.2/DX(MIND:DXD2»DXD1:DYD1»SGN(DYD)'»SQR(L2.DXD1*DXD1):DYD2»-DYD1 

5310  RETURN 

5320' 

5500  •FIND  THE  OBSTACLE  PAIRS  IN  THE  VEHICLE  PATH  THAT  ARE  TOO  CLOSE  FOR  THE 
5510  'VEHICLE  TO  PASS  BETWEEN  THEM, 

5520  RMV«R(MlNl>f  2*RVd4O0 
5530  FOR  UI  TO  NO 
5540  IF  I«MINI  THEN  5560 

5550  IF  RHO(MINIJ)<RMV-»-R(I)  THEN  NCaNC+ 1:0-1 
5560  NEXT 
5570  RETURN 
5580* 

6000  'GET  THE  BASIC  SCREEN  DIAGRAM. 

6010CLS:SCREEN  1 

6020  LINE(XVO^OO-YVOHXD  WYD):CIRCLE(XD^YD)JIV,1 

6030PORC-1TONOB0 

6040  CIRCLE(Xl(C)^00-Yl(O)Jll(C).„,l 

6050  NEXT 

6060  RETURN 

6070' 

6500  DISPLAY  THE  VEHICLE  MOVING  TO  THE  COLLISION  POINT  AT  AN  OBSTACLE  AND 
6510  'RECORD  THE  VEHICLE  PATH  POSITION  AND  RADIUS  AND  OBSTACLE  RADIUS  IN  THE 
6520  OmPUT  FILE. 

6530  DXV«(XA-XVyiO:DYV-(Y  A-YVy  10 

6540  FOR  I«1  TO  10 

6550  XX«XV:YY«YV*R-RV 

6560  IF  LAST-0  THEN  6580  ELSE  GOSUB  6000:GOSUB  7000:PR1NT  «2^.YVJKV  J1(MINI) 

6570  XV-XV+DXV:  YVxYV+DYV:IF  XV<XV0  THEN  XV-XVO:  YV-YVO 
6580  'CHECK  FOR  A  COLLISION 
6590  GOSUB  8050 

6600  GOSUB  6000:GOSUB  7000J»RINT  #2^.YV4lVJl(MINI) 

6610  NEXT 

6620  XX«XV:YY-YV  J>RINT  #2^,YVJl  V  Jl(MINI) 

6630  GOSUB  6000:GOSUB  7000-PRINT  #2^V,YV  J1V41(MINI) 

6640  RETURN 
6650' 
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7000  DRAW  THE  VEHICLE. 

7010  aRCLE(XX^YY)JWX.l 

7020  RETURN 

7030' 

7500  ’  MOVE  THE  VEHICLE  AROUND  AN  OBSTACLE. 
7510DXX»XX-X(MINI):DYY»YY-Y(MINI):DXV.XV-X(MIND:DYV=YV-Y(MINI) 

7520  R0B-SQR((X(MINI).XA)^+(Y{MINI>  YAV'l) 

7530  'CALCULATE  THE  VEHICLES  ANGULAR  POSITION  RELATIVE  TO  THE  OBSTACLE. 

7540  AB»DXX.-<MU)-DYY:GOSUB  8000:TH1=TH0 
7550  AB-DXVK»D«DYV:GOSUB  8000:TH2=TH0 
75d0  DTIMTH2-THiy5:TH«THl 
7570  FOR  0-1  TO  5 
7580TH-TH+DTH 

7590  XX-X(MINI>+ROB*CX)S(TH);YY»Y(MINI>fROB*SIN(TH) 

7600  THEN  REFRESH  SCREEN  DISPLAY. 

7610  IF  LAST-0  THEN  7620  ELSE  GOSUB  6000:GOSUB  7000:PRINT  #2;CX.YY41V  Jl(MINI) 

7620  NEXT 

7630  IF  LAST-0  THEN  7650 
7640  TF INKEYS-""  THEN  7130 
7650  RETURN 
7660' 

8000  'CALCULATE  THE  ANGLE. 

8010  THO«ATN(ORD/AB) 

8020  IF  SGN(AB)-1  THEN  TH(>=THO+3.1415926#:GOTO  8040 
8030  IF  SGN(ORD)»-l  THEN  •ra0«'ra04«.2831852# 

8040  RETURN 

8050  UHECK  FOR  A  COLLISION  WITH  ANY  OBSTACLE. 

8060  FC»  OBNUM-1  TO  NO 

8070DXY-(XV.Xl(OBNUM)y'2HYV-Yl(OBNUM)y'2DRR-(RV+Rl(OBNUM))^2 

8080  IF  .01+DXY<DRR  THEN  9000 

8090  NEXT 

8100  RETURN 

8110' 

8500  IF  S<y((X(I)-Xa-l)y'2+(Y(I>Ya*l))^2<R(I>+R(I-l)  THEN  CH*l:PRINT"OBJECT  TOO  CLOSE  TO  THE 
LAST  ONE." 

8510  RETURN 
8520' 

9000  SCREEN  2:SCREEN  0:PRINT  "COLLISION!!!":PRINT  XV;YV^l(OBNUM);Yl(OBNUM)DXY  J)RR 
9010  PRINT  #2.-l,-lJDXYDRR:PRINT  #24.  X1(I).Y1(I):PRINT  #2,"COLLISION!!!" 

9020  IF  INKEYS-""  THEN  9020 
9030  XVO=XA:YVO-YA;GOTO  9080 

9040  SCREEN  2:SCREEN  OiPRINT  "OBSTACLES  TOO  CLOSE  TO  VEHICLE  STARTING  POINT. 

9050  PRINT  "COLLISION  OCCURS  AT  X  =";XA;",  Y  =";YA;"." 

9060  IF  INKEYS-""  THEN  9060 
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9070  GOSUB  tiOOChGOTO  91 10 
9080  P0RO1T0N0B0 
9090  CIRCLE(X(C);200-Y(Q)4l(Q 
9100  NEXT 

9110  XX-XV0:YY-YV0-J»-RVK3OSUB  7000 
9120  IF  INKEY$»“"  THEN  9120 
9130  SCREEN  2:SCREEN  0:STOP 


0  PROGRAM  TO  AVOID  SPHERICAL  OBSTACLES  USING  A  FUZZY  ALGORTIUM 
10  INPUT  "USE  OBSTACLE  PARAMETERS  IN  FILE  Y(N)",ANS$ 

20  IF  ANS$«"Y"  C«  ANS$="y"  THEN  OffiN  "r,l."OBSTACLE.DAT"  ELSE  GOTO  60 
30 'YES,  SO  GET  PARAMETERS  FROM  FILE 
9  40GOSUB  1000:0010 1 10 

50  NO.  SO  GET  PARAMETERS  DIRECTLY  FROM  KEYBOARD 
60  OPEN  "0",I."OBSTACLEJ)Ar 

70  OET  VEHICLE  RADIUS  AND  ITS  INITIAL  AND  DESTINATION  (X,Y.Z)  LOCATIONS 
80GOSUB2000 

#  90  XiET  NUMBER  OF  OBSTACLES.  THEIR  RADII.  THEIR  (X.YZ)  LOCATIONS 
100  GOSUB  2500 

1 10  INPUT  "USE  MEMBERSHIP  FUNCTION  PARAMETERS  IN  FILE  Y(N)"wWS$ 

120  IF  ANS$-"Y"  OR  ANS$="y"  THEN  GOSUB  1500:GOTO  150 
130  NO  SO  ENTER  PARAMETERS  WITH  KEYBOARD 
^  140  OPEN  "O"  A"MF.DAr  :GOSUB  3000 

150  FOR  I«1  TO  NO 

160  PRINT  "OBSTACLE  #"a;":  X  «";X1(I)."Y  «";Y1(I)."Z  »";Z1(I) 

170  NEXT 

180  IF  INKEYS-""  THEN  180 

^  190 ’START  WITH  VEHICLE  IN  INITIAL  POSITION.  INITIALIZE  CONHGURATTON  STATE 

200  'AND  OPEN  FILE  TO  RECORD  VEHICLE  PATH 

210  XV«XV0:YV«YV0-.ZV=ZV0:FIRST»lLAST»0;OPEN  "0"3,"VC00RDS.DAT’ 

220  CLS  JNPUT  "BASIC  INCREMENT  FOR  VEHICLE  MOTION  ALONG  PATH";DD 
230  •GET  NUMBER  OF  DISCREET  MEMBERSHIP  FUNCTION  VALUES  FOR  FUZZY  SET  ’RATIO’ 
^  240  THAT  DEFINES  TNE  DIRECTION  OF  THE  VEHICLE  MOTION  AT  A  POINT 

250  INPUT  "NUMBER  OF  RATIO  VALUES"^fllAT:DRAT»RAT(U.2yNRAT:GOTO  420 
260  'GET  DISTANCE  OF  VEHICLE  FROM  DESTINATION 
270  DXD-XD-XV;DYD-YD-YV 

280  DXYD2rf>XD*DXDfDYD*DYD:MIND-DXYD2:DXYD-SQR(DXYD2) 

290  IF  THE  VEHICLE  IS  CLOSER  TO  THE  DESTINATION  THAN  THE  LENGTH  OF  ITS  OWN 

•  300  PADIUS  LET  THE  VEHICLE  GO  STRAIGHT  TO  THE  DESTINATION 
310  IF  DXYIXRV  THEN  XV»XD;YV*YD:DEST$-"DESr  :GOTO  420 

320  IF  NOT  THEN  FIND  THE  NEAREST  OBSTACLE.  WHETHER  mS  THE  LAST  ONE.  AND  IF 
330  TTIS  NOT  GET  THE  DIRECTION  ORTHOGONAL  TO  THE  VEHICLE’S  LINE  OF  SIGHT  TO 
340  "niE  OBSTACLE 
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350  GOSUB  3500:IF  LAST»1  THEN  390 

360  TF  THE  NEAREST  OBSTACLE  IS  NOT  THE  LAST  GET  THE  INCREMENT  FACTOR  FOR  THE 
370  ’VEHICLE  MOTION  ORTHOGONAL  TO  THAT  DIRECTION 
380  GOSUB  4000 

390  'CALCULATE  NEW  VEHICLE  POSITION  AND  SET  A  FLAG  INDICATING  THAT  IT  IS  NO 

400  'LONGER  THE  INITIAL  POSITION 

410  GOSUB  4500:IF  FIRST=I  THEN  FIRST=0 

420  'UPDATE  SCREEN  AND  SAVE  LAST  VEHICLE  POSITION  AND  DIRECTION 
430  GOSUB  9000:XX»XV:  YY»YV 

440  GOSUB  6000KX)SUB  6500:LXV=XV±YV=YV:LALPH=ALPHA:LBET=BETA 
450  IF  FRST-O  THEN  490 

460  'FREEZE  THE  SCREEN  CONHGURATTON  BEFORE  THE  VEHICLE  STARTS  TO  MOVE 
470  IF  INKEYS*’"'  THEN  470 

480  IF  NOT  AT  DESTTNATTON  GET  VEHICLE  POSITION  UPDATE 

490  IF  DESTSoDESr  THEN  270 

500  IF  INKEYS*""  THEN  500 

510  SCREEN  2:SCREEN  0 

520  END 

530’ 

1000  'GET  OBSTACLE  PARAMETERS  FROM  FILE 
1010 1-0:INPUT  #1^0.YV0ZV0JIV 
1020  INPUT  #i;KD.YD;m 
1030  WHILE  NOTXEOFd)) 

1040 

I050INPUT#l^(I).Y(D2(DJR(I):Xl(I)*X(I):Yl(I)*Y(I)2ia)=Z(I):Rl(I>=Ra) 

1060  WEND 
1070 NO*I 
1080  RETURN 
1090' 

1500  OET  MEMBERSHIP  FUNCTION  PARAMETERS  FROM  FILE 

1510  OPEN  'T'A''MF.DAr 

1520  FOR  1*1  TO  3 

1530  FOR  J*1  TO  3 

1540  INPUT  #2JlADaJ) 

1550  NEXT 
1560  NEXT 
1570  FOR  1*1  TO  3 
1580  FOR  J*1  TO  3 
1590  INPUT  #2J)ISaJ) 

1600  NEXT 
1610  NEXT 
1620 FORM  TO 3 
1630  FOR  J*1  TO  3 
1640  FOR  K«1  TO  3 
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1650  INPUT  #2  JlAXa  J  JC) 

1660  NEXT 
1670  NEXT 
1680  NEXT 
1690  RETURN 
1700’ 

2000  XiET  INITIAL  VEHICLE  DATA 

2010  INPUT  "VEHICLE  INITIAL  X.YZ  COORDINATCS";XVO,YVO^O 
2020  INPUT  "VEHICLE  RADIUS"^1V 

2030  INPUT  "DESTTNATICW  X.Y^  COORDINATES";XD.YDZD 
2040  PRINT  #1^0.YV0^0JIV  J»RINT  #1^.YD^ 

2050  RETURN 
2060* 

2500  XSET  INITIAL  OBSTACLE  DATA 
2510  INPUT  "NUMBER  OT  OBSTACLES".NO 
2520  WHILE  I<NO 
25301>l4-l 

2540  PRINT  "OBSTACLE  #";I 

2550  INPUT  "X,Y^  C00RDINATES";X(I),Y(D2(I) 

2560 INPUT  "RADIUS"ai(I):Xl(I>=X(I):Yl(I)*Y(I)21(I)=Za):Rl(I)*Ra) 

2570  TF  FILE  IS  IN  INPUT  MODE  DONT  WRITE  IN  IT 
2580  TF  FIL$«"r  THEN  2100 

2590  NEW  DATA  AND  WRITE  ONLY  FILE  MODE  SO  PRINT  PARAMETERS  IN  FILE 
2600  PRINT  «1^(D.Y(I)^JR(I) 

2610  WEND 
2620  RETURN 
2630' 

3000  XjET  fuzzy  set  parameters  for  DEHNINC  small,  medium,  or  large  FOR  THE 
3010  TIADIUS  OF  AN  OBSTACLE  AND  FOR  THE  DISTANCE  OF  AN  OBSTACLE  FROM  A  VEHICLE 
3020  'AND  THE  PARAMETERS  ASSOCIATED  WITH  THE  MEMBERSHIP  FUNCTIONS  OF  THE  MATRIX 
3030  ELEMENTS  IN  THE  MATRIX  DETERMINED  BY  THE  RULES  OF  THE  FORM  TF  THE 
3040  OBSTACLE  RADIUS  IS  AND  THE  OBSTACLE  DISTANCE  IS  *  THEN  THE  RATIO  IS ' 

3050  PRINT  ”3  PARAMETERS  DEFINING  THE  MEMBERSHIP  FUNCnONS:"J>RINT 
3060  PRINT  "'SMALL  OBSTACLE  RADIUS'--"dNPUT 

"SRADI,SRAD2,SRAD3"U1AD(1.1)JIAD(1,2)RAD(U} 

3070  WRITE  «2RAD(I.I)JIAD(U)41AD(U) 

3080  PRINT  "TkffiDIUM  OBSTACLE  RADIUS'~":INPUT 

"MRADl>fRAD2JliRAD3"aiAD(2.1)RAD(2,2)RAD(23) 

3090  WRHE  «2RAD(2.1)RAD(2^)J(AD(23) 

3100  PRINT  "lARGE  OBSTACLE.  RADIUS'-":INPUT 

"LRADl,LRAD2LRAD3"aiAD(3.1)RAD(3,2)RAD(33) 

3110  WRITE  «2RAD(3.1)RAD(3,2)RAD(33) 

3120  PRINT  "'SMALL  OBST.  DISTANCE'-":INPUT  "SDIS1,SDIS2.SDIS3";DIS(1.1)JDIS(1,2)J>IS(U) 

3130  WRITE  «2J>IS(1,1)J)IS(1,2)J>IS(1.3) 


3140  PRINT  "'MEDIUM  OBST.  DISTANCE'-.":INPUT  "MDIS1>IDIS2JMDIS3";DIS(2,1)  J)IS(2,2).DIS(23) 
31S0  WRITE  «2J)1S(2.1)  J>IS(2^)  J>IS(2.3) 

3160  PRINT  "’LARGE  OBST.  DISTANCE'-":INPUT  "LDISliJ)IS2a-DIS3";DIS(3.1)J>IS(3.2)J)IS(3,3) 
3170  WRITE  #24>IS(3.1)J>IS(3^)J)IS(3.3) 

3180  PRINT  "RATIO  IF  'SMALL  OBSTACLE  DISTANCE’  AND;" 

3190  PRINT  "’SMALL  OBSTACLE  RADIUS’-":INPUT 

"SDSRl^DSR2,SDSR3"aiAT(l.l.l)JlAT(l,U)JlAT(l,1.3) 

3200  WRITE  #23AT(1.1.1)J1AT(1.U)J1AT(1.U) 

3210  PRINT  "’MEDIUM  OBSTACLE  RADIUS’-":INPUT 

"SDMRl.SDMR2.SDMR3"UlAT(i;i,l)JlAT(U^)JlAT{U.3) 

3220  WRITE  #2JIAT(1  A1)JU^T(U^)JIAT(U3) 

3230  PRINT  "’LARGE  OBSTACLE  RADIUS’-":INPUT 

"SDLRl^DLR2.SDLR3"aiAT(U.l)JlAT(U.2)JlAT(133) 

3240  WRITE  #241AT(U.1)JIAT(U^)JIAT(1.33) 

3250  PRINT  "RATIO  IF  'MEDIUM  OBSTACLE  DISTANCE’  AND:" 

3260  PRINT  "’SMALL  OBSTACLE  RADIUS’-":INPUT 

"MDSRlJ»«DSR2JklDSR3";RAT(2.1.1)RAT(2,U)JlAT(2.1.3) 

3270  WRITE  #2JRAT(2.1.1)^T(2.U)41AT(2.U) 

3280  PRINT  "'MEDIUM  OBSTACLE  RADIUS'-":INPUT 

"MDMR1^MR2>«DMR3"UIAT(2;2.1)JIAT(2A2)JIAT(2;23) 

3290  WRITE  #2JlAT(23.1)JiAT(233)JlAT(233) 

3300  PRINT  "’LARGE  OBSTACLE  RADIUS’-":INPUT 

"MDLR13«DLR2>*DLR3"^IAT(23.1)41AT(233)JIAT(23.3) 

3310  WRITE  #2JIAT(23.1)JIAT(233)^T(233) 

3320  PRINT  "RATIO  IF  T.ARGE  OBSTACLE  DISTANCE’  AND:" 

3330  PRINT  "SMALL  OBSTACLE  RADIUS’~":INPUT 

"LDSR1JLDSR2JJ)SR3".RAT(3.1.1)4<AT(3.U)JU^T(3.13) 

3340  WRITE  #2RAT(3.1,1)J1AT(3,U)JU^T(3.13) 

3350  PRINT  "MEDIUM  OBSTACLE  RADIUS’-":INPUT 

"LDMRiaJ)MR2JLDMR3"SL\T(33.1)3AT(333)JU^T(333) 

3360  WRITE  #2RAT(33.1)JIAT(333)^T(333) 

3370  PRINT  "LARGE  OBSTACLE  RADIUS’~":INPUT 

"LDLRlJJ)LR2JLDLR3"aiAT(33,l)JUVT(333)41AT(333) 

3380  WRITE  #2JIAT(33,1)JIAT(333)41AT(333) 

3390  RETURN 
3400’ 

3500  'GET  DIRECTION  ORTHOGONAL  TO  LINE  OF  SIGHT  FROM  VEHICLE  TO  NEAREST 
3S10X>BSTACIJE 

3520  'FIRST  GET  NEAREST  OBSTACLE  AND  OBSTACLE  WITH  MAXIMUM  RADIUS 
3530  PORI-1  TO  NO 

3540  DX(I)-X(I>XV J)Ya)=Y(I)- YV:DZ(I>:3(1>ZV 

3550  DXY(I)oDX(I)*DX(I)fDYa)*DY(I>fDZ(I)*DZ(I);DRR(IHR(I)+RV)^2 

3560  IF  DXY(I)<MIND  THEN  MINI=I;MIND-DXY(I) 

3570  IF  R(I>R(MAXD  THEN  MAXI-I 
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3580  IF  LAST«1  THEN  3620 

3590  TF  NOT  ALREADY  KNOWN  TO  BE  THE  LAST  OBSTACLE  CHECK  WHETHER  IT  IS  OR  NOT 
3600  'AND  SET  -1  FLAG  IF  ITIS  NOT  BY  VIRTUE  OF  ITS  DIRECTION 
3610  IF  DX(I)*DXD+DY(I)*DYD>0  THEN  LAST*- 1 
3620  NEXT 

3630  IF  FLAG  NOT  SET  THEN  OBSTACLE  MUST  BE  THE  LAST 
3640  IF  LAST-1  THEN  LAST-0  ELSE  LAST-1 

3650  'CHECK  IF  THE  NEAREST  OBSTACLE  IS  LAST  BECAUSE  THE  VEHICLE  IS  CLOSER  TO 
3660  THE  DESTINATTON  THAN  TO  THE  OBSTACLE  AND  QUIT  IF  ITIS 
3670  IF  DXY(MINI>DXYD2  THEN  LAST-l:GOTO  3770 

3680  tJET  UNIT  VECTOR  IN  ORTHOGONAL  DIRECTION  SLANTED  TOWARD  DESTINATION 
3690  IF  ORTHOGONAL  TO  DESTINA'nON  DIRECTION,  AWAY  FROM  BIGGEST  OBSTACLE 

3700  IF  DY(MINI)o0  THEN  3730 

3710  IF  DY(MAXI)o0  THEN  ALPHA-0;BETA^-SGN(DY(MAXI)):GOTO  3770 

3720  BETA-1K30TO  3770 

3730  TH- ATN(DX(MINiyD Y(MINI)) 

3740  ALPHA-OOS(TH):BETA«SIN(TH) 

3750  DOTPtaALPHA*DXDf BETA*DYD 

3760  IF  DOTP<0  THEN  ALPHA=-ALPHA;BETA-BETA 

3770  RETURN 

3780' 

4000  'GET  FUZZY  SET  'RA'nO’  OF  MAGNITUDE  OF  VKTOR  IN  (ALPHA3ETA)  DIRECTION  TO 
4010  INCREMENT  DD  OF  VEHICLE  MOVEMENT. 

4020  'FIRST  GET  OBSTACLE  DISTANCE  FROM  VEHICLE  AND  RADIUS 
4030  ODIS-SQR(DXY(MINI)):ORAD-R(MINI) 

4040  THEN  GET  'RATIO'  MEMBERSHIP  VALUES  AND  DEFUZZIFY 
4050  RATMhNSUM-OrDSUM-O 
4060  WmLE  RATI<-RAT(U,2) 

4070  GOSUB  7000 

4080  NSUM-NSUM<('RATI*MAXMF:DSUM-DSUM-^MAXMF 
4090  RA'n-RATI-i-DRAT 
4100  WEND 

41 10  IF  NSUM-0  THEN  RA'HO-O  ELSE  RATIO-NSUM/DSUM 

4120  RETURN 

4130* 

4500  XJET  NEW  VEHICLE  POSITION 
4510  IF  LAST-1  THEN  4580 
4520DD1«RATIO*DD 

4530  IF  NEAREST  OBSTACLE  CHANGES  SPECIAL  HANDLING  REQUIRED 
4540  IF  MINIoLMINI  AND  FIRST-0  THEN  GOSUB  5000 
4550  XV-XV+DD1*ALPHA:YV=YV+DD1'»BETA 
4560  IF  DIRCH-1  THEN  4620 

4570  X3ET  DIRECTION  OF  DESTINATION  FROM  VEHICLE  AND  INCREMENT  VEHICLE  POSITION 
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4580DXI>ii«IXXV:DYI>=YI>-YV:DXYI>»S(?l(DXD*DXD+DyD*DYD);DXDO=DXD/DXYD:DYIX)*DYD/DX 

YD 

4590  ’ALONG  THAT  DIRECTION 

4600  XV»XV+DD*DXD0:  YV»YV+DD*DYD0 

4610  'GET  DISTANCE  OF  VEHICLE  IN  NEW  POSITION  FROM  NEAREST  OBSTACLE 
4620  DXY(MINIHX(MINI)-XVy'2KY(MINI)-YV)^2+CZ(MINI)- 

4630  'RECORD  THIS  DATA  IN  VEHICLE  PATH  FILE  ALONG  WITH  THE  SUM  OF  THE  VEHICLE 

4640  'AND  OBSTACLE  RADH 

4650  PRINT  #3  JCV.YVDXY(MINI)DRR(MINI) 

4660  TF  THE  DISTANCE  BETWEEN  THE  VEHICLE  AND  THE  NEAREST  OBSTACLE  IS  GREATER 
4670  THAN  THE  SUM  OF  THE  RADH  THEN  THERE  IS  NO  COLUSION 
4680  IF  DXY(MINI>DRR(MINI)  THEN  GOTO  4720 

4690  'OTHERWISE  THERE  IS  A  COLUSION  THAT  MUST  BE  AVOIDED  SO  REVERSE  THE 

4700  DIRECTION  OT  THE  VEHICLE  MOTION  AND  TRY  AGAIN 

4710  XV»LXV:YV*LYV:ALPHAs»-LALPH;BETA=-LBET:GOTO  4550 

4720  RETURN 

4730' 

5000  IF  NCmiAL  DIRECTION  BRINGS  VEHICLE  CLOSER  TO  FORMER  NEAREST  OBSTACLE 
5010  'CHANGE  DIRECnON  UNLESS  LAST  OBSTACLE  IS  NOT  TOO  CLOSE  TO  PRESENT  ONE 
5020  IFLMINIsO  THEN  5150 

5030SroEl^(LMINlHRV:SIimiR{LMINI)-RV:SIDE3=R(MINI>fRV:SIDE4=R(MINI)-RV 

5040ANGl=;ATN(Sir«2/S(»l(SIE«1^2-SIE«2''2)):ANG2*A'm(SIDE4/SQR(SIDE3''2-SIDE4^2)) 

5050ANG»3.1415926#-(ANG1+ANG2):SIDE5=SII®1A2+SIDE3^2-2*SIDE1*SIDE3*COS(ANG) 

5060  DM1NI^X(LM1N1>X(MIN1))^2-KY(LMINI>  Y(MIN0)^2+<?(LMIN^ 

5070  TF  THERE  IS  ROOM  TO  SPARE  THEN  CONTINUE 

5080  IF  SIDE5<8*DMINI  THEN  5150 

5090  IF  NOT  THEN  REVERSE  DIRECTION  AND  TRY  AGAIN 

5100TXV»XV:TYV=YV 

51 10  TXV«TXV+I»1*ALPHA:TYV*TYV+DD1*BETA:TDXD=XD-TXV:TDYD=YD-TYV 
5120  TDXYD=SQR(TDXD*TDXI>fTDYD*TDYD):TDXDO*TDXD/IDXYD:TDYDO=TDYD/rDXYD 
5130  TXV»'IXV4^DI>*TDXD0:TYV*TYV+DD*TDYD0 

5140  IF  (TXV.X(LMINI))^2+(TYV.Y(LMINI))^2<(XV.X(LMIN1))^2-KYV-Y(LMINI))^2  THEN 
ALPlLU-ALPHA:BETA»-BETA:DIRCH=l:GOTO  5160 
5150  LMINI-MINI:DIRCH^ 

5160  RETURN 
5170’ 

5500  COLUSION  OCCURRED.  SO  ANNOUNCE  IT  ON  THE  SCREEN  AND  QUIT 
5510  SCREEN  2:SCREEN  0:LOCATE  1230:PRINT  "COLUSION!!!" 

5520  PRINT  #3,-l,-l  J)XY(DDRRa):PRINT  #3  J,  X(I),Ya):PRINT  #3."COLLISION!!!" 

5530  IF  INKEYS®""  THEN  5530 

5540  XX®XV:YY*YV:GOSUB  6000:COSUB  6500 

5550  IF  INKEY$="  THEN  5550 

5560  SCREEN  2:SCREEN  OrSTOP 

5570’ 
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6000  TMIAW  BASIC  ORIGINAL  OBSTACLES.  VEHICLE  PATH  AND  DESTINATION  ON  SCREEN 
6010  CLSiSCREEN  1 

6020  LINE(XV0^00-YV0HXD.200-YD):CIRCLE(XD.200-YD)JlV.l 

6030  FOR  C»1  TO  NO 

6040  CIRCLE(X(C)  WY(C))Jl(C)„„l 

60S0NEXT 

6060  RETURN 

6070' 

6500  TMIAW  VEHICLE  POSITION  ON  SCREEN 
6510  CIRCLE(XX.200-YY)JlV.2„.l 
6520  RETURN 
6530' 

7000 1*ERFC»M  COMPOSmC»4  OF  RULES  FOR  CALCULATING  RATIO 

7010  OET  MEMBERSHIP  FUNCTION  OF  THE  INTERSECTION  OF  OBSTACLE  RADIUS'  AND 

7020  DISTANCE  FROM  VEHICLE'  FOR  ALL  COMBINATIONS  OF  RADIUS  AND  DISTANCE  SIZES 

7030MAXMF-0 

7040FORDS1ZE»1T03 

7050  'GET  OBSTACLE  DISTANCE  FROM  VEHICLE  MEMBERSHIP  FUNCTION  VALUE  MFl 
7060  PUDIS(DSIZE.l):P2-DIS(DSIZE.2):P3<=DIS(DSIZE3):IM3DIS:GOSUB  7500 
7070  MFl-MF 

7080  OET  OBSTACLE  RADIUS  MEMBERSHIP  FUNCTION  VALUE  MF2 
7090PQRRS1ZEb1T0  3 

7100  P1«RAD(RS1ZE.1)J>2«RAD(RSIZE.2):P3«:RAD(RSIZE3):PoORAD:GOSUB  7500 
7110MF2*MF 

7120  XJET  INTERSECTION  MEMBERSHIP  FUNCTION  VALUE  MFDR 
7130  IF  MF1<MF2  THEN  MF*MF1  ELSE  MF^MF2 
7140  MFDRsMF 

7150  'GET  MEMBERSHIP  FUNCTION  VALUE  FOR  RATIO  AS  CARTESIAN  PRODUCT  OF  'OBSTACLE 
7160  DISTANCE'  AND  OBSTACLE  RADIUS'  ACCORDING  TO  EACH  RULE  AND  THEN  THE  UNION 
7170 'OF  ALL  RULES 

7180Pl»RAT(DSI2ERSIZE,l):P2sRAT(DSIZERSIZEa)J*3=RAT(DSlZERSIZE3):P»RATI 
7190  GOSUB  7500 

7200  IF  MF<MFIHi  THEN  MFRDRsMF  ELSE  MFRDRsMFDR 

7210  IF  MFRDR>MAXMF  THEN  MAXMFsMFRDR 

7220  NEXT 

7230  NEXT 

7240  RETURN 

7250' 

7500  'GENERIC  MEMBERSHIP  FUNCTION  MF(P1.P2J»3) 

7510  IF  P1<P2  THEN  7530 

7520  IF  P<»P2  THEN  MF=100TO  7580  ELSE  GOTO  7550 
7530  IF  P<rfl  THEN  MIM):GOTO  7580 
7540  IF  P<P2  THEN  MFi(P-Piy(P2-Pl):GOTO  7580 
7550  IF  P<P3  THEN  MFi=(P3-P)/(P3-P2):GOTO  7580 


7560  IF  P3<P2  THEN  MF-l:GOTO  7580 

7570  IF  P>«P3  THEN  MF«0 

7580  RETURN 

9000  FOR  1-1  TO  NO 

9010  IF  DXY(I)<DRR(I)  THEN  5500 

9020  NEXT 

9030  RETURN 


0  TROGRAM  TO  AVOID  CYLINDRICAL  OBSTACLES  USING  A  FUZZY  ALGORITHM 
10  INPUT  "USE  OBSTACLE  PARAMETERS  IN  FILE  Y(N)";ANS$ 

20  IF  ANS$»"Y"  OR  ANS$»"y"  THEN  OPEN  "I".l,"OBSTACLE.DAT"  ELSE  GOTO  60 
30  TF  •YES',  GET  PARAMETERS  FROM  THE  FILE  •OBSTACLE.DAT. 

40GOSUB  lOOOrGOTO  110 

50  TF  740*.  GET  PARAMETERS  DIRECTLY  FROM  THE  KEYBOARD  AND  STORE  IN  THE  FILE: 

60  OPEN  "O'.l.’OBSTACLEDAT 

70  X3ET  THE  VEHICLE  RADIUS  AND  ITS  INITIAL  AND  DESTINATION  (X.YZ)  LOCATIONS; 
80GOSUB2000 

90  X)ET  THE  NUMBER  OF  OBSTACLES.  THEIR  RADH,  AND  THEIR  (X.YZ)  LOCATIONS. 

100  GOSUB  2500 

no  INPUT  "USE  MEMBERSHIP  FUNCTION  PARAMETERS  IN  FILE  Y(N)";ANS$ 

120  TF  •YES’.  GET  PARAMETERS  FROM  THE  FILE  'MF.DAT. 

130  IF  ANS$«"Y"  OR  ANS$=:"y"  THEN  GOSUB  1500:GOTO  160 

140  TF  ■NO*.  GET  PARAMETERS  DIRECTLY  FROM  THE  KEYBOARD  AND  STORE  IN  THE  FILE. 
150  OPEN  "0"^."MF.DAr:GOSUB  2620 
160  FORM  TO  NO 

170  PRINT  "OBSTACLE  #"a;":  X  a";Xl(I),"Y  -";Y1(D."Z  ="^1(I) 

180  NEXT 

190  IF  INKEY$»""  THEN  190 

200  •START  WITH  THE  VEHICLE  IN  ITS  INITIAL  POSITION.  INITIALIZE  THE 

210  •CONFIGURATTON  STATE.  AND  OPEN  A  FILE  TO  RECORD  THE  VEHICLE  PATH. 

220  XV«XV0:YV=YV0-.ZV=ZV0:FIRST=lLAST=0;OPEN  "0"3."VCOORDS.DAT" 

230  CLS:INPUT  "BASIC  INCREMENT  FOR  VEHICLE  MOTION  ALONG  PATH";DD 

240  •GET  THE  NUMBER  OF  DISCREET  MEMBERSHIP  FUNCTION  VALUES  FOR  THE  FUZZY  SET 

250  •RATIO'  THAT  DEFINES  THE  CORRECTIVE  CHANGE  IN  THE  VEHICLE  MOTION  DIRECTION. 

260  INPUT  "NUMBER  OF  RATIO  VALUES"J4RAT:DRAT=RAT(1  ^^VNRATrGOTO  430 

270  ■GET  THE  DISTANCE  OT  THE  VEHICLE  FROM  THE  DESTINATION. 

280  DXDteXD-XV:DYDfeYI>YV 

290  DXYD2aDXD*DXI>fDYD*DYD:MIND=DXYD2;DXYD=SQR(DXYD2) 

300  TF  THE  VEHICLE  IS  CLOSER  TO  THE  DESTTN  ATTON  THAN  THE  LENGTH  OF  ITS  OWN 
310  TIADIUS.  LET  THE  VEHICLE  GO  STRAIGHT  TO  THE  DESTINATION. 

320  IF  DXYIXRV  THEN  XV=XD:YV=YD:DESTS="DEST":GOTO  430 

330  TF  NOT  THEN  FIND  THE  NEAREST  OBSTACLE  AND  WHETHER  ms  THE  LAST  ONE.  IF 
340  TTIS  NOT  GET  THE  DIRECTION  ORTHOGONAL  TO  THE  VEHICLE'S  LINE  OF  SIGHT  TO 
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3S0  THE  OBSTACLE. 

360  GOSUB  3500:IF  LAST*1  THEN  400 

370  TF  THE  NEAREST  OBSTACLE  IS  NOT  THE  LAST  GET  THE  INCREMENT  FACTOR  FOR  THE 
380  ’VEHICLE  MOTION  ORTHOGONAL  TO  THAT  DIRECTION. 

390  GOSUB  4000 

400  CALCULATE  THE  NEW  VEHICLE  POSITION  AND  SET  A  FLAG  INDICATING  THAT  IT  IS  NO 
410  IXX^ER  THE  INITIAL  POSITION. 

420  GOSUB  4S0O;IF  FIRST*1  THEN  FIRST-0 

430  'UPDATE  THE  SCREEN  AND  SAVE  THE  LAST  VEHICLE  POSITION  AND  DIRECTION. 

440  GOSUB  8010:XX-XV:YY»YV 

450  GOSUB  6000:GOSUB  d500:LXV«XV±YV»YV±ALPH*ALPHA;LBET»BETA 
460  IF  FIRST-0  THEN  SOO 

470  'FREEZE  THE  SCREEN  CONHGURATION  BEFORE  THE  VEHICLE  STARTS  TO  MOVE. 

480  IF  INKEYS-*"  THEN  480 

490  IF  ms  NOT  AT  THE  DESTTNATTON  GET  THE  VEHICLE  POSITION  UPDATE. 

500  IF  DESTSo''DEST'  THEN  280 
510  IF  INKEYS-""  THEN  510 
520  SCREEN  2:SCREEN  0 
530  END 
540' 

1000  'GET  THE  OBSTACLE  PARAMETERS  FROM  THE  FILE  'OBSTACLE.DAT. 

1010 1-0:INPUT  #1^0.YV0ZV0JIV 
1020  INPUT  #1^.YD^ 

1030  WHILE  NOT(EOF(l)) 

1040I-I<t-l 

1050  INPUT  #1^(I),Y(I)Z(D^(I):X1(I)=X(D:Y1(I)=Y(0Z1(I)=Z(I)*1(I)=R(I) 

1060  WEND 
1070  NO-I 
1060  RETURN 
1090' 

1500  'GET  THE  MEMBERSHIP  FUNCTION  PARAMETERS  FROM  THE  FILE  ’MF.DAT. 

1510  OPEN  •I"^,"MFJ)A'r 
1520  FOR  1-1  TO  3 
1530  FOR  J=1  TO  3 
1540  INPUT  #2JRADaj) 

1550  NEXT 
1560  NEXT 
1570  FOR  1-1  TO  3 
1580  FOR  J-1  TO  3 
1590  INPUT  #24)IS(IJ) 

1600  NEXT 
1610  NEXT 
1620  FOR  I-l  TO  3 
1630  FOR  J-1  TO  3 
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1640  FOR  K«1  TO  3 

1650  INPUT  #2JlATaJ  JO 

1660  NEXT 

1670  NEXT 

1680  NEXT 

1690  RETURN 

1700' 

2000  'GET  THE  INITIAL  VEHICLE  DATA  FROM  THE  KEYBOARD. 

2010  INPUT  "VEHICLE  INITIAL  X.Y2  COORDINATES":XVO.YVOZVO 
2020  INPUT  "VEHICLE  RADIUS" JIV 

2030  INPUT  "IffiSTINATION  X.Y^  COORDINATES";XD.YDZD 
2040  'RECORD  THE  DATA  IN  THE  FILE  'OBSTACLE.DAT. 

2050  PRINT  #1  JCV0,YV0^0J1V;PRINT  #1.XD.YD;ZD 

2060  RETURN 

2070' 

2500  'GET  THE  INITIAL  OBSTACLE  DATA. 

2510  INPUT  "NUMBER  OF  OBSTACLES";NO 

2520  WHILE  I<NO 

2530I»I<»-1 

2540  PRINT  "OBSTACLE  #";I 

2550  INPUT  "X.Y2  C00RDINATES":X(I).Y(I)2(D 

2560  INPUT  "RADIUS"  Jl(I):Xl(I^X(I):Yl(I)=Y(I)21(I)=Za):Rl(I)=R(I) 

2570  'RECORD  THE  DATA  IN  THE  FILE  'OBSTACLE.DAT. 

2580  PRINT  #1  JC(D,Y(I)«I)J1(I) 

2590  WEND 
2600  RETURN 
2610' 

2620  'GET  FUZZY  SET  PARAMETERS  FOR  DEFINING  A  'SMALL',  'MEDIUM',  OR  l-ARGE' 

2630  'RADIUS  OF  AN  OBSTACLE  AND  DISTANCE  OF  AN  OBSTACLE  FROM  A  VEHICLE.  ALSO, 

2640  GET  THE  PARAMETERS  ASSOCIATED  WITH  THE  MEMBERSHIP  FUNCTIONS  OF  THE  MATRIX 
2650  'ELEMENTS  IN  THE  MATRIX  DETERMINED  BY  THE  RULES  OF  THE  FORM  TF  THE 
2660  OBSTACLE  RADIUS  IS  •  AND  THE  OBSTACLE  DISTANCE  IS  *  THEN  THE  RATIO  IS 
2670  PRINT  "3  PARAMETERS  DEFINING  THE  MEMBERSHIP  FUNCTIC»4S:":PRINT 
2680  PRINT  "'SMALL  OBSTACLE  RADIUS'-":INPUT 

"SRADl,SRAD2,SRAD3"aiAD(I,I)JlAD(U)JlAD(U) 

2690  WRITE  #2JIAD(1,1)  J1AD(U)JIAD(1,3) 

2700  PRINT  "’MEDIUM  OBSTACLE  RADIUS'-":INPUT 

"MRADI>1RAD2>IRAD3"RAD(2,I)J1AD(2,2)JIAD(23) 

2710  WRITE  #2JIAD(2,1)JIAD(2,2)JIAD(2,3) 

2720  PRINT  "T-ARGE  OBSTACLE.  RADIUS'-":INPUT 

"LRADIXRAD2,LRAD3"J1AD(3,1)J1AD(3,2)J1AD(3,3) 

2730  WRITE  #2J1AD(3.1)J1AD(3,2)RAD(3,3) 

2740  PRINT  "'SMALL  OBST.  DISTANCE’-":INPUT  "SDIS1,SDIS2,SDIS3";DIS(1,1)J)IS(1,2)J>IS(1,3) 

2750  WRITE  #2J)IS(1,1)4>IS(1,2)  jaS(U) 
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2760  PRINT  "MEDIUM  OBST.  DISTANCE'-’.INPUT  "MDIS1MDIS2MDIS3";DIS(2.1)J>IS(2^).DIS(2.3) 
2770  WRITE  #2J>IS(2.1)J)IS(2^).DIS(2.3) 

2780  PRINT  "TARGE  OBST.  DISTANCE  --";INPUT  "LDIS1TD1S2JLDIS3";DIS(3.1)DIS(3,2)DIS(3.3) 
2790  WRITE  #2J)IS(3.1)4)IS(3^),DIS(3.3) 

2800  PRINT  "RATIO  IF  ‘SMALL  OBSTACLE  DISTANCE’  AND:" 

2810  PRINT  "’SMALL  OBSTACLE  RADIUS’-":INPUT 

"SDSRl  ^  DSR2.SDSR3"^IAT(1 .1 .1)  AT(1 .1  JIAT(1 .1 .3) 

2820  WRITE  #24lAT(l.l.l)J^T(l.U)JlAT(l.U) 

2830  PRINT  "’MEDIUM  OBSTACLE  RADIUS’--":INPUT 

"SDMRl^DMR2^DMR3";RAT(U.l)RAT(U^)JtAT(U.3) 

2840  WRITE  #2^T(U.1)JIAT(U^)JIAT(1;23) 

2850  PRINT  "’LARGE  OBSTACLE  RADIUS’~":INPUT 

"SDLRl.SDLR2.SDLR3"aiAT(1.3.1)JlAT(U^)4LAT(U3) 

2860  WRITE  #2aiAT(U.l)JlAT(133)4lAT(U3) 

2870  PRINT  "RA'nO  IF  ’MEDIUM  OBSTACLE  DISTANCE’  AND:" 

2880  PRINT  "’SMALL  OBSTACLE  RADIUS’-":INPUT 

"MDSR1MDSR2MDSR3";RAT(2.1 .1)R  AT(2.1 3)ilAT(2.1 3) 

2890  WRITE  #2JlAT(2.1.1)JlATa.U)^T(2.13) 

2900  PRINT  "’MEDIUM  OBSTACLE  RADIUS’-":INPUT 

"MDMR1MDMR2>IDMR3";RAT(23.1)JIAT(233)JIAT(233) 

2910  WRITE  #2JRAT(23.1)JUVT(233)JIAT(233) 

2920  PRINT  "-LARGE  OBSTACLE  RADIUS’-":INPUT 

"MDLRl>fDLR2MDLR3";RAT(23.1).RAT(233)JlAT(233) 

2930  WRITE  #2JIAT(23.1)JIAT(233)JIAT(233) 

2940  PRINT  "RATIO  IF  T-ARGE  OBSTACLE  DISTANCE’  AND:" 

2950  PRINT  "SMALL  OBSTACLE  RADIUS’-":INPUT 

"LDSRlLDSR2JLDSR3"aiAT(3.1.1)EAT(3.U)EAT(3.13) 

2960  WRITE  #2JRAT(3,1.1)EAT(3.U)EAT(3.13) 

2970  PRINT  "MEDIUM  OBSTACLE  RADIUS’-":INPUT 

"LDMR1LDMR2LDMR3";RAT(33.1)EAT(333)EAT(3.23) 

2980  WRITE  #2 ’RAT(33.1)EAT(333)EAT(333) 

2990  PRINT  "LARGE  OBSTACLE  RADIUS’-":INPUT 

"LDLR1LDLR2LDLR3"SIAT(33.1)EAT(333)EAT(333) 

3000  WRITE  #2EAT(33,1)EAT(333)EAT(333) 

3010  RETURN 
3020’ 

3500  ’GET  THE  DIRECTION  ORTHOGONAL  TO  THE  LINE  OF  SIGHT  FROM  THE  VEHICLE  TO  THE 
3510 -NEAREST  OBSTACLE.  FIRST  GET  THE  NEAREST  OBSTACLE. 

3520  FOR  I«1  TO  NO 

3530 DXCI>*X(I)-XV:DY(I>«Y(I)-YV 

3540  DXY(I>«DX(I)*DX(D+DY(D*DY(I):DRR(IMR(I)+RVy'2 

3550  IF  DXY(D<MIND  THEN  MINI=I:MIND=DXY(I) 

3560  -IF  mS  KNOWN  TOAT  THE  LAST  OBSTACLE  IS  AT  HAND  SKIP  THE  DIRECTION  TEST. 

3570  IF  LAST«1  THEN  3620 


3580  TF  NOT  ALREADY  KNOWN  TO  BE  THE  LAST  OBSTACLE.  CHECK  WHETHER  THE  OBSTACLE 
3590  IS  OR  NOT  THE  LAST  BY  VIRTUE  OF  ITS  DIRECTION  AND  SET  THE  -1  FLAG  IF  IT 
3600  IS  NOT. 

3610  DDOT(D-  DX(I)*DXDfDY(I)*DYD;IF  DDOT(I)>0  THEN  LAST=-1 
3620  NEXT 

3630  'CALCULATE  THE  DISTANCE  BETWEEN  THE  VEHICLE  AND  THE  NEAREST  OBSTACLE. 

3640  ODIS«S(^(DXY(MIND) 

3650  TF  THE  -1  FLAG  IS  NOT  SET  THEN  THE  OBSTACLE  MUST  BE  THE  LAST  ONE. 

3660  IF  LAST-1  THEN  LAST*0  ELSE  LAST=1 

3670  'CHECK  IF  THE  NEAREST  OBSTACLE  IS  THE  LAST  BECAUSE  THE  VEHICLE  IS  CLOSER 
3680  TO  THE  DESTTNATTON  THAN  TO  THE  OBSTACLE  AND  QUIT  IF  IT  IS. 

3690  IF  DXY(MINI>DXYD2  THEN  LAST=l;GOTO  3780 

3700  'GET  THE  UNIT  VECTOR  (ALPHA3ETA)  IN  THE  DIRECTION  ORTHOGONAL  TO  THE  PATH 
3710  "FROM  THE  VEHICLE  TO  THE  NEAREST  OBSTACLE  CENTER,  SLANTED  TOWARD  THE 
3720’DESTINA'nON. 

3730  ALPHA»-DY(MINI)/ODIS:BETA=DX(MINI)/ODIS 

3740  'CHECK  TO  DETERMINE  WHETHER  THE  SLANT  OF  THE  UNIT  VECTOR  IS  CORRECT. 

3750  DOTPteALPHA*DXI>fBETA*DYD 
3760  IF  NOT  REVERSE  ITS  SIGN. 

3770  IF  DOTP<0  THEN  ALPHA- ALPHA:BETA=-BETA 

3780  RETURN 

3790' 

4000  GET  THE  RATIO  OF  THE  MAGNITUDE  OF  THE  VECTOR  IN  THE  (ALPHA3ETA) 

4010  DIRECnCW  TO  THE  INCREMENT  DD  OF  THE  VEHICLE  MOVEMENT.  FIRST  GET  THE 
4020  'RADIUS  C»AD  OF  THE  NEAREST  OBSTACLE. 

40300RAD«R(MINI) 

4040  THEN  GET  THE  FUZZY  SET  TIATTO'  MEMBERSHIP  VALUES  AND  DEFUZZIFY  THE  RATIO. 
4050  RATT-0:NSUMs0:DSUM»0 
4060  WHILE  RATToRATCU^Z) 

4070  GOSUB  7000 

4080  NSUMsNSUM+RATT*MAXMF:DSUM=DSUM-i-MAXMF 
4090  RATTaRATT-t-DRAT 
4100  WEND 

41 10  IF  NSUMaO  THEN  RATTOM)  ELSE  RATTO=NSUM/DSUM 

4120  RETURN 

4130' 

4500  GET  THE  NEW  VEHICLE  POSITION. 

4510  IF  LASTal  THEN  4610 

4520  CALCULATE  THE  SIZE  OF  THE  CORRECTION  TO  THE  VEHICLE  COORDINATES. 
4530DD1=RATIO*DD 

4540  IF  THE  NEAREST  OBSTACLE  CHANGES.  A  NEW  CALCULATION  OF  THE  CORRECTIVE 
4550  'DIRECTION  VECTOR  (ALPHA3ETA)  IS  NEEDED. 

4560  IF  MINIoLMINI  AND  FIRST=0  THEN  GOSUB  5000 

4570  INCREMENT  THE  VEHICLE  COORDINATES  BY  THE  RESULTING  CORRECTIONS. 
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4580  XV-XV+DD1*ALPHA:YV-YV+DD1*BETA 
4590  IF  DIRCH-1  THEN  4670 

4600  'GET  THE  DESTINATION  DIRECTION  RELATIVE  TO  THE  VEHICLE. 

4610DXD-XD-XV:DYI^YD-YV:DXYI>«SQR(DXD*DXI>fDYD*DYD);DXDO=DXD/DXYD:DYD0«DYDyDX 

YD 

4620  INCREMENT  THE  VEHICLE  POSITION  ALONG  THAT  DIRECHON  BY  THE  INITIALLY 
4630  'SELECTED  MAGNITUDE  DD. 

4640  XV«XV+DD*DXD0:YV«YV+DD*DYD0 

4650  'GET  THE  DISTANCE  OT  THE  VEHICLE  IN  THE  NEW  POSITION  FROM  THE  NEAREST 
4660 'OBSTACLE. 

4670  DXY(MINIHX(MINI>XV)^2+<Y(MINI>  YV)^ 

4680  'RECORD  THIS  DATA  IN  THE  VEHICLE  PATH  FILE  ALONG  WITH  THE  SUM  OF  THE 
4690  'VEHICLE  AND  OBSTACLE  RADU.  IF  THE  DISTANCE  BETWEEN  THE  VEHICLE  AND  THE 
4700 19EAREST  OBSTACLE  IS  GREATER  THAN  THE  SUM  OF  THE  RADU  THEN  THERE  IS  NO 
4710XX)LLISION. 

4720  IF  DXY(MINI>IMW(MIND  THEN  GOTO  4760 

4730  •OTHERWISE  THERE  IS  A  COLUSION  THAT  MUST  BE  AVOIDED  SO  REVERSE  THE 
4740  •DIRECTION  OF  THE  VEHICLE  MOTION  AND  TRY  AGAIN. 

4750  XV»LXV:YV«LYV:ALPHA»-LALPH:BETA«-LBET:GOTO  4580 
4760  PRINT  #3.XV,YV  J)XY(MINI)  JWIR(MIND 
4770  RETURN 
4780' 

5000  TF  NC»MAL  DIRECTION  BRINGS  VEHICLE  CLOSER  TO  FORMER  NEAREST  OBSTACLE 

5010  XHANGE  DIRECTION  UNLESS  LAST  OBSTACLE  IS  NOT  TOO  CLOSE  TO  PRESENT  ONE 

5020  IF  LMINI-0  THEN  5130 

5030  OBSEPKR(LMINI>f R(MINI>f2*RV)^2 

5040  DMINKX(LMINI>-X(MINI))^2^<YaJ»«INI>Y(Mff^ 

S050  TF  THERE  IS  ROOM  TO  SPARE  THEN  CONTINUE 
5060  IF  OBSEP<DMINI  THEN  5130 
5070  TF  NOT  THEN  REVERSE  DIRECTION  AND  TRY  AGAIN 
5080  TXV«XV:TYV- YV 

5090  T7CV»TXV+I»l*ALraA;'rYV«'rYV+DDl*BErA:'roXD«XD-'rXV;TDYD«YD.'rYV 
5100  TDXYI>i«S<»Cn>Xl>TDXIN-TDYI>TDYD):TDXDO«TDXD/rDXYD:TDYDD«TDYD/TDXYD 
51 10  T7CV«'reV4^DDTT>XD0:'rYV»'rYV+DD^YD0 

5120  IF  (TXV-X(LMINI))^^<^YV.Y(LMINI))^<(XV-X(LMINI))^24(YV-Y(LMINI)y'2  THEN 
ALPHA».ALPHA:BErA»-BETA:DIRCH«lKjOTO  5140 
5130  LMINI-MINI:DIRCH-0 
5140  RETURN 
5150* 

5500  'A  COLLISION  OCCURRED,  SO  ANNOUNCE  IT  CW  THE  SCREEN  AND  QUIT. 

5510  SCREEN  2;SCREEN  OrLOCATE  1230:PRINT  "COLLISION!  !!- 

5520  PRINT  «3.-l.-l  J>XY(I)J>RR(I)  J»R1NT  #3J,X(1),Y(I):PRINT  #3,"COUJSION!!!" 

5530  IF  INKEYS-""  THEN  5530 


5540  XX*XV:YY-YV:CiOSUB  6000:GOSUB  6500 
5550  IF  INKEY$»""  THEN  5550 

5560  SCREEN  2:SCREEN  0;STOP  , 

5570' 

6000  'DRAW  THE  OBSTACLES.  ORIGINAL  VEHICLE  PATH  AND  DESTINATION  ON  SCREEN. 

6010  CLSrSCREEN  I 

6020  LINE(XV0.200-YVOHXD.200-YD):CIRCLE(XD.200-YD)JlV.l 

6030  FOR  C»I  TO  NO  ^ 

6040  CIRCLE(X(Q.200.Y(Q)Jl(C),.„I 

6050  NEXT 

6060  RETURN 

6070' 

6500  DRAW  THE  VEHICLE  POSITION  ON  SCREEN. 

6510  CIRCLE(XX.200-YY)JIV.2„.l  ' 

6520  RETURN 

6530' 

7000  PERFORM  THE  COMPOSITION  OF  RULES  FOR  CALCULATING  THE  RATIO.  GET 

7010  THE  MEMBERSHIP  FUNCTION  OF  THE  INTERSECTION  OF  OBSTACLE  RADIUS'  AND 

7020  "OBSTACLE  DISTANCE  FROM  THE  VEHICLE'  FOR  ALL  COMBINA'HONS  OF  FUZZY  RADIUS  < 

7030  'AND  DISTANCE  SIZES. 

7040MAXMF-0 
7050F(XIDSIZE«1T0  3 

7060  'GET  THE  OBSTACLE  DISTANCE  FROM  THE  VEHICLE'  MEMBERSHIP  FUNCTION  VALUE 

7070  'MFl.  ( 

7080  PI«DIS(DSIZE.I):P2»DIS(DSIZE  J);P3*DIS(DSIZE.3):P«ODIS:GOSUB  7500 

7090MFUMF 

7100  OET  THE  OBSTACLE  RADIUS'  MEMBERSHIP  FUNCTION  VALUE  MF2. 

7110  FOR  RSIZE-I  TO  3 

7120Pl«RAD(RSIZE.l)P2sRAD(RSIZE.2):P3»RAD(RSIZE3):P»ORAD:GOSUB  7500  | 

7130MF2>MF 

7140  OET  THE  INTERSECTICW  MEMBERSHIP  FUNCTION  V/ILUE  MFDR. 

7150  IF  MF1<MF2  THEN  MF^MFI  ELSE  MF^MFZ 
7160  MFDRaMF 

7170  'GET  THE  MEMBERSHIP  FUNCTION  VALUE  FOR  PATIO'  AS  THE  CARTESIAN  PRODUCT  OF 
7180  OBSTACLE  DISTANCE  FROM  THE  VEHICLE'  AND  'OBSTACLE  RADIUS'  ACCORDING  TO 
7190  •EACH  RULE  AND  THEN  THE  UNION  OF  ALL  RULES. 

7200Pl«RAT(DSIZEPSIZE.l):P2=RAT(DSIZEpSIZE.2):P3=RAT(DSIZEPSIZE3):Pi=RA'n 
7210  GOSUB  7500 

7220  IF  MF<MFI»t  THEN  MFRDR«MF  ELSE  MFRDR=MFDR 
7230  IF  MFRDR>MAXMF  THEN  MAXMF=MFRDR 
7240  NEXT 
7250  NEXT 
7260  RETURN 
7270* 
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7500  THE  GENERIC  MEMBERSHIP  FUNCTION  MF(P1  J>2J>3). 
7510  IF  P1<P2  THEN  7530 

7520  IF  P<-P2  THEN  MFWlrGOTO  7580  ELSE  GOTO  7550 

7530  IF  Pci^i  THEN  MF-0:GOTO  7580 

7540  IF  P<3»2  THEN  MF«<P-Pl)/(P2-Pl):GOTO  7580 

7550  IF  P<P3  THEN  MF»(P3-P)/(P3-P2):GOTO  7580 

7560  IF  P3<P2  THEN  MF=l:GOTO  7580 

7570  IF  P>-P3  THEN  MF-0 

7580  RETURN 

7590' 

8000  X:HECK  FOR  A  COLUSION  WITH  ANY  OBSTACLE. 

8010  FORM  TO  NO 

8020  IF  DXY(D<DRR(D  THEN  5500 

8030  NEXT 

8040  RETURN 


0  TROGRAM  TO  AVOID  CYLINDRICAL  OBSTACLES  USING  THE  SAFOR  CRISP  ALGORITHM 
10  INPUT  "USE  OBSTACLE  PARAMETERS  IN  FILE  Y(N)"u^NS$ 

20  IF  ANS$*"Y"  OR  ANS$«"y"  THEN  OPEN  "r.l."OBSTACLE.DAT  ELSE  GOTO  60 
30  TES.  SO  GET  PARAMETERS  FROM  FILE. 

40GOSUB  500:GOTO110 

50  WO.  SO  GET  PARAMETERS  DIRECTLY  FROM  KEYBOARD. 

60  OPEN  -0".l.'OBSTACLE  DAr 

70  X3ET  THE  VEHICLE  RADIUS  AND  ITS  INITIAL  AND  DESTINATION  (X.Y)  LOCATIONS. 
80GOSUB  1000 

90  XJET  THE  NUMBER  OF  OBSTACLES.  THEIR  RADH.  AND  THEIR  (X.Y)  LOCATIONS. 

100  GOSUB  1500 
110  PORI-1  TO  NO 

120  PRINT  "OBSTACLE  #"a;":  X  -"pCl(I),"Y  »";Y1(I) 

130  NEXT 

140  IF  INKEYS-""  THEN  140 

150  ETART  Wrra  THE  VEHICLE  IN  ITS  INITIAL  POSITION.  INITIALIZE  THE 

160  CONHGURATION  STATE.  AND  OPEN  THE  FILE  TO  RECORD  THE  VEHICLE  PATH. 

170  XV-XV0:YV-YV0:FIRST-l:LAST=0;OPEN  "0"3,"VCOORDSDAr 

180  CLSrINPUT  "BASIC  INCREMENT  FOR  VEHICLE  MOTION  ALONG  PATH";DD 

190  GOTO  360 

200  XJET  THE  DISTANCE  OF  THE  VEHICLE  FROM  THE  DESTINATION. 

210  DXD-XD-XV:DYD-YD-YV 

220  DXYD2-DXD*DXDf DYD*DYD:MIND-DXYD2:DXYD=SQR(DXYD2) 

230  TF  THE  VEHICLE  IS  CLOSER  TO  THE  DESTINATION  THAN  THE  LENGTH  OF  ITS  OWN 
240  HADIUS  LET  THE  VEHICLE  GO  STRAIGHT  TO  THE  DESTINATION. 

250  IF  DXYIXRV  THEN  XV«XD:YV- YD:DEST$-"DESr  :GOTO  350 

260  TF  NOT  THEN  FIND  THE  NEAREST  OBSTACLE.  WHETHER  mS  THE  LAST  ONE.  AND  IF 


270  TT  IS  NOT  GET  THE  DIRECTION  ORTHOGONAL  TO  THE  VEHICLE'S  LINE  OF  SIGHT  TO 
280  THE  OBSTACLE. 

290  GOSUB  2000 

300  TF  THE  NEAREST  OBSTACLE  IS  NOT  THE  LAST  GET  THE  INCREMENT  FACTOR  FOR  THE 
310  •VEHICLE  MOTION  ORTHOGONAL  TO  THE  DIRECTION  OF  THE  OBSTACLE  FROM  THE 
320  •VEHICLE.  CALCULATE  THE  NEW  VEHICLE  POSITION  AND  SET  A  FLAG  INDICATING 
330  THAT  rr  IS  NO  LONGER  THE  INITIAL  POSITION. 

340  GOSUB  2S00 

350  UTOATE  SCREEN  AND  SAVE  LAST  VEHICLE  POSITION  AND  DIRECTION 

360XX»XV:YY«YV 

370  GOSUB  3S00:GOSUB  4000 

380  IF  FIRST*0  THEN  420 

390  •FREEZE  THE  SCREEN  CONHOURATION  BEFORE  THE  VEHICLE  STARTS  TO  MOVE 
400  IF  INKEYS*--  THEN  400 
410  IF  FIRST*1  THEN  FIRST=0 

420  TF  NOT  AT  DESTINATION  GET  VEHICLE  POSITION  UPDATE 

430  IF  DESTSo-DEST  THEN  210 

440  IF  INKEYS*-  THEN  440 

450  SCREEN  2:SCREEN  0 

460  END 

470  • 

500  •GET  OBSTACLE  PARAMETERS  FROM  FILE 
510 1-0:INPUT  #  W0.YV02V0JRV 
520  INPUT  #1^,YD;ZD 
530  WHILE  NOT(EOF(I)) 

540I*RI 

550  INPUT  #I^(I),Y(I);2(I)JI(I);X1(I>X(I):Y1(I)=Y(I)*1{I)=R(I) 

560  WEND 
570NO*I 
580  RETURN 
590' 

1000  GET  INITIAL  VEHICLE  DATA 

1010  INPUT  -VEHICLE  INTTIAL  X.YZ  COORDINATES";XVO,YVOZVO 

1020  INPUT  -VEHICLE  RADIUS".RV 

1030  INPUT  -DESTINATION  X.Y  COORDINATES  ";XD.YDZD 

1040  PRINT  #1^0,YV0;eV04lV:PRINT  #1^D.YDZD 

1050  RETURN 

1060' 

1500  GET  INITIAL  OBSTACLE  DATA 
1510  INPUT  -NUMBER  OF  OBSTACLES";NO 
1520  WHILE  I<NO 
1530I-I+1 

1540  PRINT  -OBSTACLE  #-;I 

1550  INPUT  -X.YZ  C00RDINATES-;X(I),Y(I)2(I) 
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1560  INPUT  -RADIUS"ai(D:Xl(I)»X(D:Yl(I^Ya)  JUa>=R(I) 

1570  TF  FILE  IS  IN  INPUT  MODE  DONT  WRITE  IN  IT 
1580  TF  FIL$-T  THEN  2 100 

1590  'NEW  DATA  AND  WRITE  ONLY  FILE  MODE  SO  PRINT  PARAMETERS  IN  FILE 
1600  PRINT  #l^(I).Y®«I)Jia) 

1610  WEND 
1620  RETURN 
1630' 

2000  'GET  DIRECTION  ORTHOGONAL  TO  LINE  OF  SIGHT  FROM  VEHICLE  TO  NEAREST 
2010  'OBSTACLE  FIRST  GET  THE  NEAREST  OBSTACLE  AND  CHECK  IF  ITIS  THE  LAST. 

2020  FOR  I«1  TO  NO 

2030  DX(I)*X(I)-XV  J)Y(I).Y(I)-YVJ)RR(IHRV+R(I)y'2 

2040DXY2(I>-DX(irDX(IHDY(D*DY(D:DXY(D=SQR(DXY2(I))J>lST(DsDXY(I)-R(D 
2050  IF  (DIST(I)<MIND)  THEN  MINI^LMINDsDISTa) 

2060  DCPA(I>=DX(D*DXD+DY(I)*DYD 
2070  IF  LAST-1  THEN  2110 

2080  IF  NOT  ALREADY  KNOWN  TO  BE  THE  LAST  OBSTACLE  CHECK  WHETHER  IT  IS  OR  NOT 
2090  'AND  SET  -1  FLAG  IF  ITIS  NOT  BY  VIRTUE  OF  ITS  DIRECTION. 

2100  IF  DCPA(I)>0  THEN  LAST-1 
21 10  NEXT 

2120  TF  THE  FLAG  NOT  SET  THEN  OBSTACLE  MUST  BE  THE  LAST. 

2130  IF  LAST-1  THEN  LAST-0  ELSE  LAST-1 

2140  'CHECK  IF  THE  NEAREST  OBSTACLE  IS  LAST  BECAUSE  THE  VEHICLE  IS  CLOSER  TO 
2150  THE  DESTINATION  THAN  TO  THE  OBSTACLE  AND  QUIT  IF  ITIS. 

2160  IF  DXY2(MINI>DXYD2  THEN  LAST=l.<X)TO  2270 

2170  GET  UNIT  VECTOR  IN  THE  DIRECTION  ORTHOGONAL  TO  THE  VEHICLE  PATH  DIRECTION 
2180  'AND  POINTING  AWAY  FROM  THE  OBSTACLE  CENTER.  GET  THE  DISTANCE  DACPA  TO 
2190  THE  OBSTACLE  CENTER  AT  THE  CLOSEST  POINT  OF  APPROACH,  THE  DISTANCE  DTCPA 
2200  TO  THE  CLOSEST  POINT  OF  APPROACH.  AND  THE  ALLOWED  PASSING  DISTANCE  PD. 

2210  DACPAKDXD*DY(MIN1).DYD*DX(MINI))/DXYD 
2220  DTCPA»DCPA(MINI)/DXYD:PD=SQR(2*DRR(MINI)) 

2230  'NOW  CALCULATE  THE  SCALE  FACTOR  SCF  AND  DETERMINE  THE  VEHICLE  COORDINATE 
2240  INCREMENTS  INCX,  INCY. 

2250  SCF»SGN(DACPA)*(PD-ABS(DACPA))*DD/ABS(DTCPA) 

2260  INCX«DY(MINI)*SCF/DXY(MINI):INCY-DX(MINI)*SCF/DXY(MINI) 

2270  RETURN 
2280' 

2500  'GET  THE  NEW  VEHICLE  POSITION.  IF  THE  LAST  OBSTACLE  IS  PASSED  SKIP  THE 
2510  TTRST  INCREMENT.  OTHERWISE.  INCREMENT  THE  VEHICLE  POSITION  ORTHOGONAL 
2520  TO  THE  DIRECHON  OF  THE  NEAREST  OBSTACLE  RELATIVE  TO  THE  VEHICLE. 

2530  IF  LAST-1  THEN  2550 

2540  IF  DCPA(MINI)>0  THEN  XV=XV+INCX:YV=YV+INCY 

2550  GET  THE  DIRECTTON  OF  THE  DESTINATION  FROM  THE  VEHICLE  AND  THE  VEHICLE 
2560  TOSinW  AFTER  INCREMENTING  ITS  COORDINATES  IN  THAT  DIRECTION. 


2570DXD»0CD-XV:DYD=YD-YV:DXYDt:SC?l(DXD*DXI>DYI>*DYD):DXDft=DXD/DXYD;DYD0=DYD/DX 

YD 

2580  XV1«XV+DD*DXD0;YV1«YV+DD*DYD0 

2590  'GET  THE  SQUARED  DISTANCE  OF  THE  VEHICLE  IN  THE  NEW  POSITION  FROM  THE 
2600  'NEAREST  OBSTACLE. 

2610  DXY21(MINIHX(MINIVXVI)^2-KY(MINI).YViy'2 

2620  IF  THE  DISTANCE  IS  LARGE  ENOUGH  ACCEPT  THE  LAST  INCREMENT  IN  THE  VEHICLE 
2630'POSrnON. 

2640  IF  DXY21(MINI)>DRR(MINI)  THEN  XVaXVl:YV=YVl 

2650  'RECXXID  THIS  DATA  IN  THE  VEHICLE  PATH  RLE  ALONG  WITH  THE  SQUARED  SUM  OF 
2660  THE  VEHICLE  AND  OBSTACLE  RADU. 

2670  PRINT  #3;CV.YV  J>XY2(MINI)J>RR(MINI) 

2680  'CHECK  FOR  A  COLUSION. 

2690  GOSUB  4500 
2700  RETURN 
2710' 

3000  'A  COLLISION  OCCURRED.  SO  ANNOUNCE  IT  ON  THE  SCREEN  AND  QUIT. 

3010  SCREEN  2:SCREEN  OiLOCATE  1230:PRINT  -COLUSION!!!" 

3020  PRINT  #3.-l.-l.-l.-l  J*RINT  #3."OOLLISION!!!- 
3030  IF  INKEYS*-  THEN  3030 
3040  STOP 
3050' 

3500  DRAW  BASIC  ORIGINAL  OBSTACLES.  VEHICLE  PATH  AND  DESTINATION  ON  SCREEN 
3510  CLS:SCREEN  1 

3520  UNE(XV0.200-YVO>(XD.200-YD):CIRCLE(XD200-YD)41V.1 

3530  FOR  C«1  TO  NO 

3540  C1RCLE(X(Q.200-Y(Q)3(Q«„1 

3550  NEXT 

3560  RETURN 

3570' 

4000  DRAW  VEHICLE  POSITION  ON  SCREEN 
4010  CIRCLE(XX.200-YY)JlV.2„.l 
4020  RETURN 
4030* 

4500  XHECK  FOR  A  COLLISION  WITH  ANY  OBSTACLE. 

4510PORI*1TONO 

4520  IF  DXY2(I)<DRR(1)  THEN  3000 

4530  NEXT 

4540  RETURN 


0  'PROGRAM  TO  REPLOT  VEHICLE  PATH  AROUND  OBSTACLES 
10  LASTX-10:LASTY*100 


D-34 


20  INPUT  "VEHICLE  COORDINATES  FILENAME"; VF$ 

30  INPUT  "OBSTACLE  DATA  FILENAME":OF$ 

40  OPEN  "r,l."OBSTACLEDAr 
50  OPEN  "I"  A"VOOORDS,DAr 
60  GOSUB  1000KK)SUB  5000 
70  WHILE  NOr(EOF(2)) 

80  INPUT  #2^.Y  J)UMMY  J5UMMY 
85  TF  A  COLLISKW4  THEN  ANNOUNCE  IT  AND  QUIT 
90  IF  X-1  AND  Y-1  THEN  GOSUB  500:STOP 
100  LINE(LASTX^OO-LASTY>OC200-Y) 

1 10  LASTX-XiASTY«Y 
120  WEND 

130  IF  INKEYS--"  THEN  130 
140  SCREEN  2:SCREEN  0 
150  END 
160' 

500  XXILLISION  TO  ANNOUNCE 
510  IF  INKEYS--  THEN  510 
520  INPUT  #2.CS 

530  SCREEN  2:SCREEN  OzLOCATE  1230:PRINT  C$ 

540  IF  INKEYS-- THEN  540 

550  RETURN 

560' 

1000  'GET  OBSTACLE  PARAMETERS  FROM  FILE 
1010 1-0:INPUT  #1^0.YV02V0J1V 
1020  INPUT  #1^,YD2D 
1030  WHILE  NOT(EOF(l)) 

1040I-I+1 

1050INPUT#l^(I).Y(I)«D.Ra):Xl(D=X(I):Yl(I^Y(I)21(I)=Z(I);Rl(I)-R(I) 

1060 WEND 
1070 NO-I 
1080  RETURN 
1090' 

5000  TXIAW  BASIC  ORIGINAL  OBSTACLES.  VEHICLE  PATH  AND  DESTTNATTON  ON  SCREEN 
5010  CLSrSCREEN  1 

5020LINE(XV0.200-YVOHXD.200-YD):CIRCLE(10.100),10.2:CIRCLE(310,100),10,1 

5030  FOR  C-1  TO  NO 

5040  CIRCLE(X(C).200-Y(Q)4I(Q„„1 

5050  NEXT 

5060  RETURN 

5070' 


DATA  FILES  P(»  USE  IN  THE  AVOIDANCE  PROGRAMS  (TO  USE  OBSTACLE  DATA  FOR  ANY 
OBSTACLE  CONHGURATION  NUMBER  N  COPY  THE  FILE  NAMED  OBSTACLN.DAT  TO 
OBSTACLEJ>AT.  WHEN  THE  PROGRAM  ASKS  "USE  PARAMETERS  IN  FILE  ?"  ANSWER  "Y"). 


OBSTACLlJ>AT 

10 

100 

10 

10 

310 

100 

10 

100 

125 

20 

20 

ISO 

70 

2S 

2S 

200 

70 

2S 

2S 

OBSTACUJ>AT 

10 

100 

1' 

10 

310 

100 

10 

100 

12S 

20 

20 

ISO 

70 

2S 

2S 

200 

70 

2S 

2S 

2S0 

120 

30 

30 

OBSTACL3J)AT 

10 

100 

10 

10 

310 

100 

10 

7S 

100 

2S 

2S 

12S 

120 

20 

20 

12S 

70 

30 

30 

200 

70 

2S 

2S 

22S 

120 

2S 

2S 

270 

80 

30 

30 

OBSTACUJ)AT 

10 

100 

10 

10 

310 

100 

10 

7S 

60 

2S 

2S 

100 

no 

30 

30 

12S 

120 

2S 

2S 

17S 

80 

30 

30 

22S 

120 

2S 

2S 

OBSTACLSDAT 

10 

100 

10 

10 

310 

100 

10 

SO 

90 

2S 

2S 

ISO 

no 

30 

30 

2S0 

120 

2S 

2S 
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MFJ>AT  cnns  IS  FUZZY  MEMSmiSHIP  FUNCTION  DATA) 

334;20 

S;203S 

2035^ 

1003030 

2030.100 

SO.IOO3O 

133 

3.103 

3.103 

5.1.3 

133 

3.103 

5.13 

5.1.3 

133 
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